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ABSTRACT 


A  six  degree  of  freedom  manipulator,  a  PUMA  560,  is  calibrated 
using  three  different  measurement  systems  in  order  to  improve  the 
accuracy  of  the  manipulator.  Closed  loop  kinematic  chain  modeling 
theory  is  presented.  Variations  in  the  models  for  each  calibration 
method  are  presented.  A  simulation  study  is  conducted  to  determine 
feasibility  of  the  proposed  methods.  Experimental  data  is  obtained 
and  the  actual  calibration  performed.  A  comparative  analysis 
between  both  simulation  and  experiment  and  between  measurement 
systems  is  performed.  Various  aspects  regarding  measurement  system 
modelling  are  discussed.  The  calibrated  kinematic  parameters  are 
presented  as  results. 
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INTRODUCTION 


There  are  two  main  objectives  that  are  addressed  in  this 
thesis.  The  first  objective  addresses  development  of  practical 
manipulator  calibration  methods.  A  number  of  different  devices 
and  techniques  have  been  employed  to  calibrate  manipulators. 
However,  most  methods  involve  highly  sophisticated,  delicate 
and  expensive  measurement  systems  which  are  well  suited  for 
laboratory  work  but  are  not  practical  in  an  industrial 
environment.  The  second  objective  addresses  problems 
associated  with  modelling  measurement  systems  within  closed 
^ oop  kinematic  chains. 

The  goal  of  calibration  is  to  improve  the  accuracy  of  the 
manipulator.  Accuracy,  in  the  sense  used  here,  is  the  ability 
of  a  manipulator  to  achieve  a  commanded  position  and 
orientation,  pose  for  short,  of  its  end  effector.  The  end 
effector  pose  is  a  function  of  both  fixed  geometric  properties 
of  the  robot,  such  as  link  lengths,  and  variable  geometric 
properties,  such  as  angular  displacement  of  a  rotary  joint. 
The  kinematic  model  is  developed  from  both  the  fixed  and 
variable  geometric  properties  and  in  a  qualitative  sense, 
these  models  are  both  well  understood  and  well  defined. 
However,  errors  between  the  pose  predicted  by  a  model  and  the 
pose  achieved  by  a  typical  manipulator  have  been  shown  by 
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experiment  to  be  10  mm  or  more  [Ref  1].  These  errors  are  due, 
in  most  part,  to  differences  between  the  design  or  nominal 
values  of  the  geometric  properties  of  the  manipulator  and  the 
actual  manufactured  values. 

Another  measure  of  a  manipulator's  performance  is  its 
repeatability.  Repeatability  is  the  ability  of  a  manipulator 
to  achieve  an  identical  pose  each  time  it  is  commanded  to  a 
specific  pose.  Current  experimentation  shows  that  manipulators 
have  a  repeatability  on  the  order  of  0.3  mm  [Ref  2]. 
Therefore,  a  measure  of  the  success  of  calibration  is  a  model 
with  an  accuracy  which  approaches  the  manipulator's 
repeatabi 1 ity . 

There  are  four  basic  steps  in  the  calibration  process 
[Ref.  3]  and  these  steps  are  described  as  follows: 

-  A  closed  chain  kinematic  model  of  the  manipulator  and 
measurement  system  is  developed.  During  this  process, 
identifiable  parameters  are  determined  and  the  measured 
quantity  or  quantities  are  specified.  A  set  of  error 
functions  are  derived  from  the  difference  in  the  measured 
quantities  and  the  quantities  predicted  by  the  model. 
Nominal  parameter  values  are  provided  by  manipulator 
manufacturing  specifications,  measurement  system 
specifications  and  the  location  of  the  measurement  system. 

-  Next,  experimental  measurements  are  taken.  These 
measurements  are  a  function  of  the  actual  parameter 
values.  Corresponding  joint  variable  data  is  incorporated 
into  the  measurement  set. 

-  Identification  of  the  parameters  is  performed  utilizing 
the  experimental  data.  This  process  consists  of 
systematically  adjusting  the  nominal  parameters  until  the 
model  predictions  match  the  experimental  data  and  hence 
the  error  functions  become  zero. 
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-  The  final  step  involves  incorporating  the  identified 
parameters  into  the  software  used  to  control  the 
manipulator . 

The  first  three  steps  were  performed  on  a  PUMA  560  six  degree 
of  freedom  manipulator  arm  utilizing  three  different 
measurement  systems.  The  first  of  these  methods  corresponds  to 
a  laboratory  method  and  uses  a  Coordinate  Measuring  Machine 
for  full  pose  measurement.  This  highly  accurate  method 
provides  a  benchmark  for  the  other  two  methods.  Although  step 
four  of  the  process  is  not  performed,  computer  simulation  of 
the  process  is  conducted  to  quantify  the  success  of  the 
calibration  method. 

Although  standardized  and  reliable  approaches  to  kinematic 
modelling  of  manipulators  exist,  closed  chain  models 
incorporating  measurement  systems  for  calibration  are  less 
well  understood.  Difficulties  arise  from  the  issue  of 
identif iability  of  parameters.  This  problem  was  studied  in 
detail  with  the  intent  of  producing  a  standardized  approach 
which  would  eliminate  the  ambiguity  often  encountered.  Due  in 
part  to  the  unlimited  number  and  type  of  measurement  systems 
available,  no  one  independant  method  is  possible.  However,  a 
systematic  approach  to  the  problem  which  alleviates  most  of 
the  difficulties  is  proposed.  Several  case  studies  are 
presented  which  not  only  illustrate  this  approach,  but 
emphasize  some  of  the  subtleties  encountered  in  modelling 
measurement  systems. 
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II. 


THEORY 


A.  CLOSED  LOOP  KINEMATIC  CHAIN  MODELING 


1 .  General  Coordinate  System  Transformations 

A  large  class  of  manipulators  can  be  thought  of  as  a 
series  of  links  connected  by  either  rotary  or  prismatic 
joints.  Typical  kinematic  models  consist  of  fixed  coordinate 
frames  attached  to  each  of  the  links  and  a  set  of 
transformation  eguations  between  these  coordinate  frames.  This 
section  will  develop  generalized  coordinate  frame 
transformations.  The  following  sections  will  then  address 
standardized  transformations  followed  by  a  development  of  the 
kinematic  model  for  the  PUMA-560.  The  following  conventions 
will  be  used  throughout  this  document: 

-  Bold  lower  case  letters  will  refer  to  vectors.  A  preceding 
superscript  refers  to  the  frame  the  vector  is  associated 
with.  A  subscript  identifies  the  frame  in  which  the 
coordinates  of  the  vector  are  referenced. 

-  Bold  upper  case  letters  will  refer  to  matrices. 

-  Upper  case  letters,  excluding  F,  correspond  to  points. 
Preceding  superscripts  and  subscripts  have  an  identical 
meaning  as  defined  for  vectors. 

-  Coordinate  frames  will  be  denoted  by  where  the 

superscript  refers  to  an  assigned  number  or  designation. 

-  Double  subscripted  lower  case  letters  will  usually  refer 
to  a  vector  or  point  of  the  same  letter.  The  first 
subscript  refers  to  the  component  of  the  vector  or 
coordinate  of  the  point  and  the  second  subscript  refers  to 
the  frame  to  which  it  is  referenced. 
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Consider  the  coincident  coordinate  frames  of  Figure  1  in 
which  the  y  and  z  axis  have  been  rotated  an  angle  i|i  about  the 
X  axis.  First,  the  io,  jo  and  ko  unit  vectors  in  the  nonrotated 
frame  will  be  described  with  respect  to  the  rotated  coordinate 
frame  unit  vectors  ij,  ji  and  kj.  This  will  then  provide  a 
method  of  describing  the  coordinates  of  point  P,  with 
coordinates  p,o,  Pyo  and  p,o/  given  the  coordinates  of  point  P, 
p^j ,  p^i  and  p,i,  with  respect  to  the  rotated  axis.  Clearly, 
rotation  about  the  x  axis  does  not  alter  the  unit  vector  or 
the  X  component  of  P.  Consequently,  this  problem  can  be 


reduced  to  a  planar  analysis  by  projection  onto  the  y-z  plane. 


Figure  1.  Effect  of  Rotation  of  a  Coincident  Coordinate  Frame 


About  the  X  Axis 


Figure  2  illustrates  this  projection.  Recalling  that 
io  and  i,  are  identical  and  noting  the  geometry  of  Figure  2, 
io,  jo  and  ko  in  terms  of  ij,  j^  and  k,  are  given  by  Equation  1. 
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(1) 


■^0  “ 

=  cos^j^  -  sini|rjq 
JCq  =  sin^r^j^  +  cos  'i/K 

Rewriting  Equation  1  in  matrix  form  results  in  the  following 

expression • 


\1' 

1 

0 

C 

=  jo 

COSl|f 

-siniiJ 

k-. 

[0 

sinilf 

COSl|( 

Equation  2  can  now  be  used  to  transform  the  coordinates  of 
point  P  with  respect  to  the  rotated  axis  into  coordinates  in 
the  nonrotated  coordinate  system  by  substituting  the 
coordinates  of  point  P  for  the  unit  vectors  as  shown  in 
Equation  3. 


Pyc\ 

I 

PzC ' 


jO  COS^f 
j^O  siniji 


0 

-sintji 

cos\)j 


(3) 


Summarizing,  the  above  3x3  matrix  can  be  interpreted  in  two 
ways.  First,  the  matrix  columns  describe  the  orientation  of 
the  rotated  frame  with  respect  to  the  nonrotated  frame.  For 
example,  the  column  2  elements  indicate  that  the  rotated  y 
axis,  which  is  described  by  unit  vector  j^,  has  direction 

=  Oio  +  cos^jo  +  sin4»*o  (4) 

Rewriting  Equation  4 , 

=  Oij  +  cos  +  cos  (90-ijf)  ic^  (5) 

and  noting  from  Figure  2  that  the  angle  90-i|(  is  the  angle 
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between  the  Zo  and  axis,  then  it  can  be  seen  that  the  column 
components  are  the  familiar  direction  cosines.  Secondly,  the 
matrix  can  be  thought  of  as  a  coordinate  transformation  matrix 
in  which  coordinates  of  points  in  one  frame  can  be 
"transformed"  into  coordinates  in  a  second  frame  as 
illustrated  in  Equation  3.  These  two  interpretations  of  the 
3x3  matrix  transformation  matrix  will  hold  for  all 
transformation  matrices  to  follow. 


J 

2^.  /I 

V  / 

r 

y\  \ 

A\  \  „ 

/ 

Figure  2 .  Planar  View  of  a  Coordinate  Frame  Rotated  About  its 
X  Axis 


If  the  rotated  coordinate  frame  is  now  rotated  about 
the  yi  axis  an  angle  6,  then  in  a  similar  manner  to  the 
preceding  analysis,  the  problem  reduces  to  analysis  in  the  x^- 
Zi  plane.  Referring  to  Figure  3,  the  i^,  and  k,  unit  vectors 
in  terms  of  the  rotated  axis  x^,  y^  and  are  given  by 
Equation  6.  These  equations  are  again  rewritten  in  matrix  form 
as  given  in  Equation  7. 
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ij^  =  cosOij  +  sinSiCj 
=  J2 

=  -sineij  +  cosQk^ 


(6) 


i 

cos6  0 

sir.0' 

i 

:/ 

= 

G  1 

0 

k 

-sin0  0 

COS0 

K 

Pre-multiplying  both  sides  of  Equation  7  by  the  matrix  in 
Equation  2  as  shown  in  Equation  8,  will  result  in  a 
transformation  between  the  original  coordinate  frame  and  the 
twice  rotated  coordinate  frame. 


U 

10  o' 

COS0  0  sin0 

= 

0  cosilf  -sini|f 

0  10 

[K 

0  sini|f  cosi|i  j 

-sin0  0  COS0 

The  preceding  analysis  demonstrates  a  valuable  property  of 
transformation  matrices.  Further  rotations  of  a  coordinate 
frame  can  be  referenced  to  the  original  coordinate  frame  by 
post  multiplying  any  previous  transformation  matrices  by  the 
transformation  matrix  that  describes  the  next  rotation. 

A  development  similar  to  that  leading  to  Equations  2 
and  7  for  a  rotation  about  the  z  axis  by  angle  0  results  in 
the  following  transformation  matrix  R, . 


R 


Z 


cos4»  -sin4>  0 
sin(}>  cos4>  0 
0  0  1 


(9) 


In  addition  to  rotation  of  a  coordinate  frame  about 
its  axis,  translation  of  the  frame  must  be  accounted  for. 
Consider  the  coordinate  frame  with  origin  at  point  P  in  Figure 
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Figure  3.  Planar  View  of  the  Effect  of  Rotating  a  Coordinate 
Frame  About  its  Y  Axis 

4.  Let  R  be  the  transformation  matrix  with  elements  r^  that 
corresponds  to  rotation  of  a  coordinate  frame  with  origin  at 
P  and  axis  originally  aligned  with  the  coordinate  frame  at  0. 

'  A  point  Q  with  coordinates  and  q,i  with  respect  to  the 

rotated  coordinate  frame  can  be  expressed  in  coordinates  with 
respect  to  point  0  by  Equation  10. 

^22  ^23 
^32  -^33, 

It  would  be  convenient  if  this  translation  transformation  were 
incorporated  in  matrix  form.  This  can  be  accomplished  by 
forming  an  augmented  4x4  matrix  and  augmented  vectors  as 
illustrated  in  Equation  11.  Matrices  of  this  form  are 
t  typically  referred  to  as  homogeiieous  transformation  matrices. 


<?xj! 


jPx 

+  \Py 


(10) 
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Figure  4 .  A  Rotated  and  Translated  Coordinate  Frame 


’^o 

X 

O. 

X 

X 

0 

X 

q 

X. 

Uy  Oy  ay  P^. 

a.  p^ 

Z‘ 

.  -  . 

0  C  0  1  , 
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The  upper  left  3x3  submatrix  has  the  same  orientation 
interpretation  as  noted  earlier  where  the  n,  o  and  a  elements 
are  the  direction  cosines  for  the  rotated  x,  y  and  z  axis  with 
respect  to  the  original  axis  respectively.  The  upper  3 
elements  of  column  four  of  the  matrix  define  the  origin  of  the 
rotated  and  translated  coordinate  frame  with  respect  to  the 
original  frame.  With  this  interpretation  in  mind,  the 
convention  Ti’  will  be  used  when  referring  to  a  transformation 
from  frame  i  to  frame  j.  When  no  subscript  is  indicated,  T’, 
then  the  transform  is  interpreted  as  being  from  a  known 
reference  frame,  which  will  be  clear  from  the  context,  to  the 
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j“  coordinite  frame.  Additionally,  the  previously  described 
3x3  rotational  transformation  matrices  can  be  expressed 
individually  as  4  by  4  homogeneous  transformation  matrices  as 
well.  The  convention  Rot(x,\ii),  Rot(y,6)  and  Rot(z,0)  will  be 
used  in  the  following  discussion  and  these  matrices  are  shown 
in  Equations  12  through  14. 


ri  0  0  Cl 

I 

jC  ccsTi;  -sini|r  o 
ic  sinil'  cosij;  0 
;  r  0  c  1 


'  COS0  0  sine  O' 
0  1  0  o' 

i-sin6  0  cosB  0 
'  0  0  0  1.' 


(  Z,  4)) 


cos4)  -sin4)  0  O' 
sinct)  cos(})  0  0 
0  0  1  Cj 

0  0  0  IJ 


(12) 


(13) 


(14) 


Additionally,  a  standard  translation  transformation  will  be 
denoted  by  Trans (x,y,z)  and  has  the  form  shown  in  Equation  15 
where  the  upper  3x3  rotational  submatrix  has  been  replaced 
with  'i  3x3  identity  matrix.  If  the  coordinate  frame  is  only 
translated  in  one  direction,  say  x,  then  the  symbol  Trans(x) 
will  be  used  to  denote  this  transformation  and  the  p^  and  p, 
terms  in  Trans(x,y,z)  are  set  to  zero. 

Given  two  coordinate  frames  F°  and  F',  the  homogeneous 
transformation  matrix  To'  and  Xi  in  F',  then  Xo  can  be  found  by 
Equation  16. 
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Trans {x, y,  z) 


(15) 


loop,' 

0  10  p^. 

0  0  1  p, 

0  0  0  1. 

(16) 


If  is  known  and  desired,  then 

(To)--  Xo  =  to  JCi 

(To^)-^X,  =x,  (17) 

Ti  ^0  = 

This  of  course  requires  knowledge  of  the  inverse 
transformation  matrix.  However,  as  described  by  Paul  [4],  if 
a  transformation  matrix  T  has  the  elements  of  Equation  18 
then  its  inverse  is  evaluated  by  Equation  19  where  n,  o,  a  and 
p  are  the  four  column  vectors  of  T  and  is  the  usual  vector 
dot  product  operator. 


Ox 

3x 

Px 

Uy 

°y 

Py 

n. 

o. 

a. 

Pz 

0 

0 

0 

Hy 

-pn 

Ox 

Oz 

-po 

Ox 

Oy 

Oz 

-pa 

0 

0 

0 

1 

J 

(18) 


(19) 


As  noted  earlier,  given  transformations  To^  and  Tj^, 
then  the  transformation  from  F°  to  F""  can  be  calculated  by  post 
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multiplying  by  This  product  can  then  be  designated  as 

or  simply  as  T-  where  the  reference  frame  F"  is  assumed. 
Figure  5  illustrates  various  transformations  by  use  of  a 
directed  graph.  Each  node  represents  a  coodinate  frame  and  the 
directed  paths  represent  transformations  in  the  given 
direction . 


0 

T  ’  =  T  ' 

•  0  • 

► 

1 

T  '  =  T  ’  T  'T  ' 

■  •  0  ■  1  ■  2 

T/ 

Y 

Y 

3 

T/ 

2 

t;  =  (t; ) ’ 

Figure  5.  Directed  Transformation  Graph 


2.  Roll,  Pitch,  Yaw  and  Translation  Transformations 

To  transform  between  two  coordinate  frames  fixed  in 
space  as  shown  in  Figure  6  requires,  in  general,  3  rotations 
and  3  translations.  Noti.ng  that  the  order  in  which  the 
transformations  occur  is  important,  adoption  of  one  of  the 
standards  will  help  avoid  confusion.  The  standard  used  for 
this  type  of  orientation  transformation  jn  this  work  is  roll, 
pitch  and  yaw. 

1  3 


Figure  6.  Roll,  Pitch,  Yaw  and  Translation  Transformation 
Between  Two  Fixed  Coordinate  Frames 

The  roll,  pitch  and  yaw  rotation  transformation  will 
be  denoted  as  RPY((^),0  ,i|»)  and  is  a  product  of  the  previously 
described  rotation  matrices  as  follows 

RPY{^,e,i\i)  =  Rot{i>,z)  RctiQ,y)  i^ot(4r,x)  (20) 
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Carrying  out  the  indicated  matrix  multiplication,  the  elements 
of  RPY(0,0,\|j)  are  given  in  Equation  21  where  cosine  and  sine 
are  denoted  by  c  and  s  for  brevity. 


c4>c^ 

cxt)s6siji-s4> 

C(|)s6cijr  +  s<t)si|f 

s4)c6 

S(t)s65»lj  +  c(})cr); 

-s6 

c6c4r 

0 

0 

0 

With  the  orientation  now  specified  by  RPY(0,6,i)f)  ,  it  is  only 
necessary  to  specify  the  translations  between  the  two  frames. 
This  can  be  accomplished  by  multiplying  RPY(0,6,(1;)  by 
Trans ( X ,y, z ) .  Note  that  premultiplication  by  Trans(x,y,z) 
implies  that  the  translations  occur  on  with  respect  to  F°  axis 
and  that  post  multiplication  implies  that  the  translations 
occur  with  respect  to  the  rotated  axis.  The  convention  used 
throughout  this  work  is  post  multiplication.  Summarizing,  the 
transform  Tq^  is  calculated  by 


To  =  RPY{^,Q,}\i)  Trans  {x,y,  z)  (22) 

=  Rot  {i> ,  z)  Rot  {6 ,  y)  Rot{}\i ,  z)  Trans  {x,  y,  z) 

with  each  step  in  the  transformation  illustrated  graphically 

in  Figure  6.  Transformations  of  this  form  will  be  denoted  by 

RPYT(0,6 , X ,y , z )  or  simply  RPYT. 

3 .  Denavit-Hartenburg  Transformations 

As  indicated  in  the  preceding  section,  3  axis 

rotations  and  3  translations  are,  in  general,  required  to 

transform  between  two  coordinate  frames.  However,  the  geometry 

of  successive  links  of  a  manipulator  imposes  constraints  on 

the  transformation  between  coordinate  frames  fixed  in  these 


15 


links  and  a  subsequent  reduction  in  the  number  of  rotations 
and/or  translations  required.  The  form  of  these 
transformations  is  a  function  of  the  link  geometry,  the  type 
of  interconnecting  joints  and  the  placement  (orientation  and 
position)  of  the  link  frames.  Clearly  a  systematic  approach  to 
frame  allocation  is  desirable  if  not  essential.  One  widely 
accepted  systematic  approach  is  the  Denavit-Hartenburg  method. 

Typical  manipulators,  such  as  the  PUMA-560  illustrated 
in  Figure  7,  consist  of  a  series  of  links  and  joints.  An  n 
degree  of  freedom  manipulator  will  have  n  links  and  n  joints. 
The  links  and  joints  of  the  manipulator  are  labeled  in  the 
following  manner.  The  first  joint  is  labeled  1  and  the  joint 
number  is  incremented  by  one  for  each  successive  joint.  Link 
i  lies  between  joint  i  and  joint  i+i.  The  base  or  base  link  is 
defined  as  link  0. 

Figure  8  illustrates  a  generic  link.  The  parameter 
a„  is  the  common  normal  distance  between  joint  axis  n  and  n+1 
and  is  usually  referred  to  as  the  link  length.  A  plane,  normal 
to  a„  at  the  intersection  of  the  common  normal  a^  and  joint 
axis  n+1  will  by  definition  contain  joint  axis  n+1  and  lines 
parallel  to  joint  axis  n.  The  angle  between  joint  axis  n+1  and 
a  line  parallel  to  joint  axis  n  in  the  plane  is  designated  a„, 
and  is  generally  referred  to  as  the  link  twist  angle.  In 
addition  to  a„,  common  normal  a„.i  intersects  joint  axis  n.  The 
distance  between  these  two  common  normals  along  the  axis  is 
designated  d„  and  is  usually  referred  to  as  the  distance 
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Figure  7.  The  PUMA  560  6  DOF  Manipulator 


between  the  links  or  the  offset  distance.  Similarly,  is 
the  offset  distance  along  joint  axis  n+1  between  common 
normals  a^  and  a„*i.  A  fourth  parameter  0„,  is  defined  as  the 
angle  in  a  plane  perpendicular  to  joint  axis  n  between  common 
normals  a„.i  and  a^. 

With  these  parameters  identified,  assignment  of 
coordinate  frames  to  each  link  based  on  these  parameters  can 
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be  accomplished.  Figure  9  illustrates  the  process  for  the  case 
of  revolute  joints.  Frame  n,  F",  lies  at  the  intersection  of 
a^,  the  common  normal  between  joint  axis  n  and  n+1,  and  joint 
axis  n+1.  If  the  joint  axis  intersect,  then  the  intersection 
is  chosen  as  the  origin  which  is  consistent  with  the  above 
description  noting  that  a„  is  zero.  If  the  axes  are  parallel, 
then  the  frame  origin  is  chosen  so  that  the  offset  distance  is 
zero  for  the  next  defined  frame  origin.  The  coordinate  frame 
axis  are  aligned  as  follows.  z„  lies  on  joint  axis  n+1.  The  x^ 
axis  is  aligned  with  a„  when  it  exists.  If  a^  does  not  exist, 
as  in  the  case  of  intersecting  joint  axis,  x„  is  aligned 
perpendicular  to  joint  axis  n  and  n+1.  The  zero  position  of 
the  joint  variable,  d„,  is  defined  when  x„.j  and  x^  are  both 
parallel  and  in  the  same  direction. 
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Figiire  9.  Frame  Allocation  Between  Rotary  Joints 

Before  proceeding  with  an  explanation  of  the  method  of 
assigning  coordinate  frames  for  prismatic  joints,  a  simple 
illustration  of  a  fundamental  difference  between  prismatic  and 
rotary  joints  is  offered.  Consider  a  point  P  traveling  in  a 
circular  path  in  space  as  illustrated  in  Figure  10a.  The  path 
of  P  defines  a  plane  and  hence  a  perpendicular  direction. 
Furthermore,  the  center  of  the  circle  in  the  above  plane 
clearly  defines  a  point  in  space  with  which  to  reference  point 
P.  On  the  other  hand,  linear  motion  of  P  as  shown  in  Figure 
10b  offers  no  such  reference  point.  In  fact,  the  axis  is 
indistinguishable  from  any  other  parallel  axis. 
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Figure  10.  Identifiable  Features  Resulting  from  Circular  and 
Linear  Motion 

Figure  11  illustrates  a  prismatic  joint  located 
between  two  rotary  joints.  For  the  prismatic  joint,  the  joint 
distance  d„  is  the  joint  variable.  As  noted  in  the  preceding 
paragraph,  the  position  of  the  joint  axis  is  undefined  and 
only  the  direction  of  the  axis  is  known.  Consequently,  the 
common  normal  parameter  a^  is  meaningless.  With  this  in  mind, 
the  origin  of  F"  is  placed  coincident  with  the  next  defined 
link  coordinate  frame  origin.  Note  that  this  placement  may  be 
ambiguous  if  the  prismatic  joint  were  at  or  near  the  end  of  a 
serial  link  manipulator  and  an  alternative  placement  may  be 
necessary.  When  placed  at  the  next  defined  origin,  the  axis 
is  aligned  with  joint  axis  n+1.  The  x„  axis  is  positioned 
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perpendicular  to  and  the  prismatic  joint  axis  n.  The  zero 
position  for  prismatic  joint  is  defined  when  the  distance  d„ 
in  Figure  11  is  zero. 


Figure  11.  Frame  Allocation  for  Prismatic  Joints 

With  the  manipulator  placed  in  the  zero  position  for 


both  rotary  and  prismatic  joints  as  defined  in  the  preceding 
paragraphs,  positive  sense  of  rotations  can  be  defined  and 
then  the  appropriate  sense  of  all  the  z  axis  determined. 
According  to  Paul  [Ref.  5],  the  base  link  frame  of  the 
manipulator,  F°,  will  be  coincident  with  the  origin  of  F\ 
However,  such  an  allocation  will  not  afford  a  standard 
transformation  from  F*^  to  F*  unless  joint  axis  1  and  2  happen 
to  intersect.  A  more  appropriate  location  of  the  base  frame 
will  be  to  place  the  Zq  axis  coincident  with  joint  axis  1  and 


in  a  direction  satisfying  the  right  hand  rule.  The  Xo  axis 
should  be  aligned  with  the  common  normal  between  joint  axis  1 
and  joint  axis  2.  If  joint  axis  1  and  2  intersect,  then  Xo 
should  lie  perpendicular  to  the  two  axes.  Note  that  this 
method  is  generally  consistent  with  the  previously  described 
method  with  the  exception  that  d.  is  defined  zero.  This 
definition  of  F°  is  not  necessary  but  other  locations  may  lead 
to  difficulty  in  determining  identifiable  parameters  as  will 
be  discussed  later.  Finally,  the  end  link  coordinate  frame, 
link  6  for  a  6  degree  of  freedom  manipulator,  is  placed 
coincident  with  the  preceding  frame  and  with  the  z  axis 
aligned  with  the  previous  frame's  z  axis.  For  a  given  set  of 
joint  variables  and  parameters,  the  end  link  frame  is  fixed  in 
space.  With  this  in  mind,  a  6  degree  of  freedom  homogeneous 
transformation,  such  as  Roll,  Pitch,  Yaw  and  Translation,  is 
then  necessary  to  describe  the  pose  of  an  end  effector  not 
coincident  with  the  end  link  frame. 

Now  that  the  pose  of  each  link  frame  is  defined,  the 
transformation  matrices  between  frames  can  be  developed.  The 
type  and  order  of  the  rotation  and  translation  transformations 
which  form  the  overall  transformation  from  frame  to  frame 
follows  in  a  natural  way  from  the  path  from  F""^  to  F". 
Referring  back  to  Figure  9,  F"'*  is  rotated  6„  about  z^.^  so  that 
rotated  x^.j  is  aligned  in  the  direction  of  a^.  The  rotated 
frame  is  then  translated  d„  in  the  z„-j  direction  followed  by 
a  translation  a„  in  the  rotated  x„.,  direction.  The  rotated  and 
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twice  translated  frame  now  only  requires  alignment  of  its  z 
axis  with  joint  axis  n+1  which  is  accomplished  by  a  rotation 
about  the  rotated  x„.i  axis,  which  now  is  equivalent  to  x„,  an 
angle  a„.  In  equation  form,  can  be  expressed  by 

~  Rotiz.Q)  Tians(0 ,0 .  d)  Tzansia.O  ,Q)  Rot(x,a)  (23) 
Carrying  out  the  indicated  matrix  multiplication, 

cosG  -sinScosa  sinGsina  acosG 
sinG  cosGcosa  -cosGsina  asinG 
0  sina  cosa  d 

0  0  0  1 

For  a  prismatic  joint,  reduces  to  the  matrix  in  Equation 

25. 

fcosG  -sinGcosa  sinGsina  O'! 

IsinG  cosGcosa  -cosGsina  0l 
T„i  =  i  .  ,!  (25) 

i  0  sino  cosa  d, 

10  0  0  ij 

4.  Modified  Denavit-Hartenburg  Transformations 

In  general,  application  of  the  Denavit-Hartenburg 
method  will  result  in  an  accurate  model  of  a  manipulator. 
However,  some  limitations  exist  such  as  the  previously  noted 
potential  ambiguity  with  regard  to  prismatic  joints  and  a 
disproportionate  model  [Ref.  6]. 

A  proportional  model  can  be  defined  as  one  in  which 
changes  in  any  model  parameter  will  result  in  changes  in  other 
model  parameters  of  the  same  order  of  magnitude.  This  is 
clearly  not  the  case  with  Denavit-Hartenburg  when  modelling 
parallel  or  nearly  parallel  consecutive  joints.  Consider  the 


(24) 
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two  nearly  parallel  intersecting  axis  lying  in  a  plane  P  in 
Figure  12.  A  small  rotation  of  axis  uu'  in  the  plane  P  will 
result  in  a  large  change  in  the  location  of  the  point  of 
intersection.  Large  changes  in  the  location  of  the  common 
normal  a„  between  nearly  parallel  axis  can  occur  in  much  the 
same  way.  Furthermore,  small  variations  in  orientation  between 
nearly  parallel  axis  may  actually  place  them  parallel  in  which 
case  a  unique  common  normal  no  longer  exists.  These 
potentially  large  variations  in  the  location  of  the  common 
normal  corresponds  to  equally  large  changes  in  the  parameter 
d„  in  the  Denavit-Hartenburg  method.  It  should  be  noted  that 
this  apparent  flaw  in  the  Denavit-Hartenburg  method  is  only  an 
issue  when  developing  a  kinematic  model  for  calibration  where 
the  fixed  model  parameters  become  variables  and  are  perturbed 
numerically.  Disproportionate  changes  will  frequently  result 
in  numerical  instability,  an  issue  which  will  be  addressed  in 
greater  detail  in  following  sections.  A  model  developed  for 
the  sole  purpose  of  determining  the  end  effector  pose  with 
respect  to  a  given  reference  frame,  the  so  called  forward 
kinematic  solution,  will  have,  excluding  the  joint  variables, 
fixed  values  based  on  assumed  geometry  and  therefore  immune  to 
problems  of  proportionality.  Of  course  the  validity  of  the 
model  is  only  as  good  as  the  geometric  assumptions  and  this 
provides  the  motivation  for  calibration. 

The  following  modification  to  the  standard  Denavit- 
Hartenburg  transformations  will  result  in  a  proportionate 
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Figure  12.  Illustration  of  Disproportionate  Length  Variation 
Due  to  Small  Axis  Rotation 

model  for  consecutive  revolute  joints.  This  modification 
follows  one  proposed  by  Hayati  and  Mirmirani  [Ref.  7].  Rather 
than  specifying  a  common  normal  distance  between  two  parallel 
or  nearly  parallel  joint  axis  n-1  and  n,  define  a  plane  that 
is  perpendicular  to  joint  axis  n-1  and  located  at  the  origin 
of  F”*’  as  illustrated  in  Figure  13.  The  intersection  of  this 
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plane  and  joint  axis  n  defines  the  origin  of  F"  which  is 
defined  whether  the  axis  are  parallel  or  not.  As  shown,  a 
rotation  about  z„.^  an  angle  0  will  place  the  rotated  axis 
on  the  line  and  thus  becomes  the  first  transformation. 

Translation  along  O^-iO^  a  distance  r„  will  place  the  origin  of 
the  rotated  and  translated  coordinate  frame  coincident  with 
.  In  general,  rotation  about  two  different  axes  are 
required  to  align  the  third  axis  of  a  frame  in  some 
arbitrarily  specified  direction.  Therefore,  to  align  the  z 
axis  of  the  rotated  and  translated  frame  with  joint  axis  n+1, 
which  maintains  continuity  with  the  standard  Denavit- 
Hartenburg  method,  rotations  about  the  x  and  y  axis  are 
required.  Equation  26  summarizes  the  above  transformations  in 
equation  form. 

Tb-i  =  Rot  (  z,  6  Trans  (r^,  0 , 0)  Rot  (x,  a  Rot  (}',  fi  (26) 
Carrying  out  the  indicated  matrix  multiplication  of  Equation 
26  results  in  the  matrix  elements  given  in  Equation  27  where, 
for  brevity,  c  and  s  have  been  substituted  for  sine  and  cosine 
respectively . 

l'-sosPs0-^cPc6  -casd  sacPs0  +  sPc6  rcO’ 

„  sasPc0-^cPs9  cac0  -sacPc6+sp5r6  rs0’ 

-cosp  sa  cocpo 

[  0  0  0  1 

Comparing  Equation  26  and  Equation  23  it  can  be  seen 
that  a  transformation  T  as  in  Equation  28  will  satisfy  any  one 
of  the  three  manipulator  transformations.  This  is  accomplished 
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-  setting  B  equal  to  zero  for  standard  Denavit-Hartenburg 
transformations  between  revolute  joints; 

-  setting  the  parameters  B  and  a  to  zero  for  standard 
Denavit-Hartenbur  transformations  for  prismatic  joints; 

-  setting  the  parameter  d  to  zero  for  the  Modified  Denavit- 
Hartenburg  transformation; 

T  =  Rot {z,Q)  Trans {z)  Trans (x) Rot ix, Rot iy , ( 28 ) 


This  transform  T  is  the  standard  transform  used  in  this  work 
for  manipulator  link  to  link  transformations.  The 
transformation  allows  for  standardizing  parameter  information 
in  tabular  form  as  well  as  a  single  subroutine  computer  code 
for  manipulator  transformations.  The  elements  of  this 
transform  are  given  in  Equations  29  where  ti,  is  an  element  in 
the  i*^  row  and  j*”  column  of  T.  Any  future  reference  to  a 
transform  between  links  of  a  manipulator  will  be  considered  to 
be  in  the  form  of  Equation  29. 


t,j  =  cos0cosp-sin6si/:!aasinp 
=  sinScosa 

=  cos  theacsinP+sinSsinacosP 
=  acosG 

=  sin6cosP+cos6sinasinP 
=  cosScosaj 

=  sin6sinp-cos6sinocosp 
=  asine  '  ^ 

t,,  =  -cosasinP 
tj.  =  sina 
£33  =  cosacp 

^34  ~  ^ 

^41  “  ^42  “  ^43  “  ^ 

^44  ~  ^ 

5.  Other  Special  Cases 

Up  to  this  point,  two  specific  types  of  transformation 
matrices  have  been  developed,  the  Roll,  Pitch,  Yaw  and 
Translation  matrix,  RPYT,  and  the  modified  Denavit-Hartenburg 
transformation  matrix.  These  matrices  will  be  used  exclusively 
for  the  kinematic  models  developed  in  this  thesis.  However, 
there  are  cases  when  transformations  must  be  described  between 
coordinate  frames  and  other  less  well  defined  geometric 
quantities.  For  example,  suppose  a  measurement  system  somehow 
clearly  defines  a  point  M  in  space  but  fails  to  define  a  set 
of  axis.  To  incorporate  this  measurement  system  into  the  model 
of  the  manipulator,  it  is  necessary  to  develop  a  transform 
from  the  point,  M,  to  a  frame  in  the  manipulator,  normally  F°, 
as  illustrated  in  Figure  14.  There  are  no  axes  to  align  nor 
are  there  axes  on  which  to  translate  from  M  to  the  origin  of 
F°.  However,  a  number  of  alternatives  are  available  to 
transform  from  F°  to  M.  Any  set  of  three  of  the  six  variables 
of  RPYT  can  be  used  to  perform  such  a  transformation  so  long 
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as  one  of  the  variables  is  a  translation.  A  transformation 
satisfying  the  preceding,  So",  can  be  formed  from  RPYT  by 
setting  the  irrelevant  variables  to  zero.  Such  a  transform 
fixes  a  frame  at  point  M  which  has  axis  orientation  dependant 
on  both  F°  and  So".  A  transformation  from  M  to  F°  can  then  be 
calculated  by  inverting  So".  A  frame  defined  in  the  manner 
above  will  be  denoted  with  italicized  pr ' .  so  as  to 
distinguish  it  from  an  independently  defined  frame. 


Figure  14.  Frame  to  Point  Transformation 
6.  The  Kinematic  Chain 

The  path  through  the  series  of  frames  illustrated  in 
Figure  15  can  be  thought  of  as  a  kinematic  chain  where  the 
transforms  between  frames  are  analogous  to  links.  As  described 
earlier,  the  pose  of  the  last  frame  in  the  chain  with  respect 
to  the  origin  of  the  chain  can  be  formed  by  post  multiplying 
each  transform  in  sequence  as  shown.  Before  applying  this 


29 


concept  to  a  manipulator,  it  should  be  noted  that  it  is 
frequently  convenient  to  define  an  external  reference  frame  in 
the  manipulator  workspace  since  F°,  as  defined  earlier,  will 
normally  be  internal  to  the  manipulator  structure  and  not 
easily  measured  or  referenced  from  the  manipulator  workspace. 
In  the  following  experimental  work,  this  external  reference 
frame  is  coincident  with  the  measurement  system  reference 
frame  and  is  referred  to  as  F”.  If  F”  is  a  fully  defined  and 
independent  frame  then  all  six  parameters  of  RPYT  are 
necessary  to  transform  from  F"  to  F°  and  this  transformation, 
T„°  becomes  the  first  link  in  the  chain.  The  following  links 
in  the  chain  are  described  by  appropriate  forms  of  the 
modified  Denavit-Hartenburg  transformations.  Recalling  that 
the  last  frame  in  the  manipulator  in  accordance  with  the 
Denavit-Hartenburg  method  is  placed  coincident  with  the 
previous  frame,  then  a  RPYT  transform  is  required  to  transform 
from  the  last  Denavit-Hartenburg  frame  to  a  frame  located  at 
the  end  effector,  F^.  The  pose  at  F^  with  respect  to  F”  denoted 
Tm*^  or  simply  T*"  can  be  calculated  by  Equation  30. 

To' •  •  •  tA  r/ 

7.  The  Thirty  Parameter  Puma  Kinematic  Model 

As  noted  by  Mooring,  Roth  and  Driels  [Ref.  8],  the 
number  of  parameters  N  in  a  complete  model  is 

N  =  4R  +  2P  +  6  (31) 
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Figure  15.  The  Kinematic  Chain 


where  R  is  the  number  of  rotary  joints  and  P  is  the  number  of 
prismatic  joints.  Complete  in  this  sense  means  that  the  model 
contains  a  fully  defined  independent  external  reference  frame 
and  an  independently  defined  tool  frame.  The  PUMA-560  consists 
of  6  rotary  joints  which  by  Equation  31  suggests  that  the 
complete  model  will  have  30  parameters.  The  PUMA-560  30 
parameter  model  is  developed  in  the  following  Section  and  the 
actual  frame  locations  are  illustrated  in  Figure  16. 

The  location  of  the  external  or  measurement  system 
reference  frame  is  arbitrary  within  the  manipulator's 
workspace.  All  six  parameters  of  an  RPYT  transformation  will 
be  required  to  transform  from  F”  to  F°.  At  this  point,  it  will 
be  useful  to  distinguish  between  joint  parameters  and  joint 
variables  as  used  in  calibration.  The  joint  variable,  denoted 
01,  is  associated  with  the  amount  joint  i  is  rotated  from  its 
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Figure  16.  PUMA-560  Frame  Allocation 


zero  position  as  defined  by  a  joint  encoder.  The  joint 
parameter,  denoted  66 i  is  essentially  the  error  between  the 
encoder  zero  and  the  actual  zero  position  of  the  i*"^  joint  as 
defined  by  the  Denavit-Hartenburg  method.  In  the  calibration 
process,  the  value  of  the  joint  variable  for  a  given  position 
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is  considered  fixed.  The  joint  parameter  or  error  is  to  be 
determined  from  its  nominal  value  which  of  course  is  zero. 
With  this  in  mind,  the  transformation  would,  in  part, 

consist  of  a  rotation  Rot  (  Zq,  661+61 )  which  is  equivalent  to  the 
product  of  Rot(Zo/56i)  and  Rot(Zo,6i).  The  first  rotation, 
Rot(Zo,66i),  is  between  two  fixed  frames.  Denoting  the 
intermediate  frame  as  F°' ,  then  a  RPYT  transformation  made  up 
of  six  fixed  parameters,  Th°',  could  be  developed  which  would 
be  equivalent  to  the  product  of  T„°  and  Rot(Zo,66i)  which  is 
made  up  of  7  fixed  parameters.  Therefore  66,  is  not 
independent  and  cannot  be  individually  identified.  As  a  final 
note,  F°  is  considered  to  be  at  the  actual  zero  position,  F°' 
in  the  preceding  discussion,  T„°  is  a  transformation  between 
F”  and  F'^  as  now  defined.  Additionally,  one  reason  for  defining 
the  parameter  do  to  be  zero  is  t*^  eliminate  the  dependency 
that  clearly  would  exist  between  the  z  translation  in  Tm°  and 
any  subsequent  translation  do  along  the  same  z  axis  in  To'. 

As  described  earlier,  F°  is  placed  with  the  Zo  axis 
coincident  with  joint  axis  1  and  directed  upward  in  accordance 
with  the  right  hand  rule.  Joint  axis  1  and  2  are  nominally 
coincident  and  perpendicular.  F'  is  allocated  in  accordance 
with  standard  Denavit-Hartenburg  with  Zi  aligned  with  joint 
axis  2  and  Xi  perpendicular  to  joint  axis  1  and  2.  With  this 
allocation,  parameters  ao  and  Oo  are  nominally  zero  and  -90“ 
respectively.  Joint  axis  2  and  3  are  nominally  parallel  so  F^ 
is  assigned  using  the  modified  Denavit-Hartenburg  method.  As 
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illustrated  in  Figure  16,  this  places  external  to  the 
manipulator.  Due  to  the  orthogonal  nature  of  the  axis  thus 
far,  02,  a2  and  are  all  nominally  zero  with  a^  nominally 
431.85  mm  as  shown.  Joint  axis  3  and  4  are  nominally 
perpendicular  and  offset  a  perpendicular  distance  of  nominally 
20.33  mm  which  become  a^  in  The  z  axis  of  F^  is  aligned 

with  joint  axis  4  with  an  offset  distance,  d^,  of  149.09mm. 
Note  that  as  illustrated,  link  3  is  shown  with  63  equal  to 
approximately  90°.  Therefore,  a^  is  iii  the  negative  x 
direction  and  Oj  is  nominally  90°.  The  z  axis  of  F‘  is  placed 
in  the  direction  of  joint  5  which  is  nominally  coincident  with 
and  perpendicular  to  joint  axis  4.  Consequently,  6,  and  a,  are 
nominally  zero  and  a,  is  -90°  with  the  chosen  direction  of  z,. 
The  distance  between  F’  and  F*  along  joint  axis  4,  which 
corresponds  to  d,,  is  433.0  mm.  F®  is  placed  so  that  z^  lies  in 
the  direction  of  joint  axis  6  which  is  again  nominally 
perpendicular  to  joint  axis  5.  Therefore,  65,  d^  and  as  are 
nominally  zero  and  is  nominally  90°  for  the  chosen 

direction  of  Z5.  According  to  standard  Denavit-Hartenburg 
methodology,  a  sixth  frame  would  be  placed  with  its  origin 
coincident  with  the  origin  of  F^.  This  would  then  be  followed 
by  a  six  parameter  transformation  to  the  end  effector  frame. 
However,  in  a  similar  manner  as  before,  the  parameters  of  Tg® 
would  be  dependant  on  all  four  parameters  of  T^®  and  hence  not 
independently  identifiable.  Therefore,  F^  will  be  considered 
the  last  link  frame  and  T^^  will  be  a  RPYT  transformation 
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containing  a  rotation  about  equivalent  to  the  sum  of  the 
joint  variable  0^,  joint  parameter  (SS^  and  any  other  fixed 
rotation  necessary  for  proper  alignment  at  the  end  effector. 

Table  1  summarizes  the  parameters  for  the  five 
modified  Denavit-Hartenburg  transformations.  A  tabular 
presentation  of  this  form  is  usually  referred  to  as  a 
kinematic  parameter  table.  The  bold  elements  comprise  the  18 
identifiable  parameters  of  the  manipulator.  The  other  elements 
are  zero  as  defined  previously.  The  additional  12  parameters 
of  the  30  parameter  model  are  the  variables  of  T„°  and  T^*^ 
which  are  dependant  on  the  location  of  the  external  reference 
frame  and  the  geometry  of  a  particular  end  effector.  The 
parameters  of  Table  1  were  utilized  in  each  of  the  following 
experiments.  The  makeup  of  T„°  and  Tg^  will  be  described  for 
each  specific  case. 

TABLE  1.  PUMA  560  KINEMATIC  PARAMETER  TABLE 


T 

er 

di  (mm) 

aj  (mm) 

o 

B/ 

0-1 

0.0 

0.0 

0.0 

-90.0 

0.0 

1-2 

0.0 

0.0 

431.85 

0.0 

0.0 

2-3 

0.0 

149.09 

-20.33 

90.0 

0.0 

3-4 

0.0 

433.00 

0.0 

-90.0 

0.0 

4-5 

0.0 

0.0 

0.0 

90.0 

0.0 
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B.  NUMERICAL  SOLUTIONS  UTILIZING  IMSL  ROUTINE  ZXSSQ 
1 .  Introduction 

The  IMSL  routine  ZXSSQ  is  a  Levenberg-Marquardt 
algorithm  for  the  solution  of  non-linear  least  squares 
problems.  The  general  problem  statement  follows 

-  Minimize:  fi(x)^  +  fj(x)^  +  .  .  .  +  fnCx)^ 

-  Over:  x  =  [  Xj,  Xj,  •  •  •  ,  x^  ] 

At  the  n'^^  iteration,  an  estimation  of  x"‘^  is  calculated  using 
a  numerical  estimate  of  the  Jacobian.  The  Jacobian  estimate  is 
calculated  by  a  forward  or  central  finite  difference  method. 

The  routine  requires  a  user  supplied  function  for 
calculation  of  the  fi(x)  functions-  An  initial  estimate  of  x 
is  supplied  to  the  routine  by  the  main  or  calling  program 
along  with  convergence  criteria.  Three  convergence  criteria 
are  available: 

-  NSIG:  The  first  convergence  criteria  is  satisfied  if  on 
two  successive  iterations,  the  parameters  agree  to  NSIG 
significant  digits. 

-  EPS:  The  second  convergence  criteria  is  satisfied  if  the 
residual  sum  of  squares  for  two  successive  iterations  is 
less  than  EPS. 

-  DELTA:  The  third  convergence  criteria  is  satisfied  if  the 
euclidean  norm  of  the  estimated  gradient  is  less  than 

DELTA  - 

Satisfaction  of  any  of  the  three  criteria  will  halt  program 
execution  and  a  number  of  parameters  are  returned  to  the 
calling  program  including  the  final  estimate  of  x,  the  final 
value  of  each  fi(x),  the  residual  sum  of  squares  in  variable 
SSQ  and  the  satisfied  convergence  criteria.  Three  variations 
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of  the  algorithm  are  selectable  by  defining  option  parameter 
lOPT.  If  the  residual  sum  of  squares  is  close  to  zero  then 
setting  lOPT  to  zero  (Brown's  algorithm  without  strict 
descent)  will  usually  perform  satisfactorily.  This  setting  of 
lOPT  was  used  for  all  applications  of  the  routine  due  to  the 
problem  formulation. 

The  general  program  flow  is  illustrated  in  Figure  17. 
An  initial  estimate,  x'',  is  supplied  from  the  calling  program 
along  with  several  parameters  including  the  convergence 
criteria  and  algorithm  option.  The  routine  then  calls  the  user 
supplied  subroutine  N  times  where  N  is  the  number  of  elements 
of  X  in  order  to  calculate  the  finite  difference  gradient 
approximations.  ZXSSQ  then  calculates  a  new  estimate,  and 
then  calls  the  user  supplied  routine  to  calculate  fjx"*').  The 
process  repeats  until  any  one  of  three  convergence  criteria  is 
satisfied . 

Although  mathematically  equivalent,  two  different 
formulations  of  the  problem  statement  for  implementation  of 
ZXSSQ  are  used  in  this  thesis.  Each  formulation  is  described 
in  the  following  two  sections. 

2 .  Data  Fitting 

In  the  calibration  process,  measurement  of  the  end 
effector  pose,  full  or  partial,  are  made  and  then  this  data  is 
used  to  adjust  or  fine  tune  the  kinematic  model  parameters.  A 
simplistic  view  of  this  problem  consists  of  holding  the  usual 
model  variables  fixed  and  varying  the  constants.  A  simple 
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Figure  17.  General  Program  Flow  for  Implementation  of  ZXSSQ 


example  follows. 

Suppose  the  mathematical  model  for  a  physical  process 
is  assumed  to  have  the  form  of  Equation  32. 

g(x,y)  =  +  a^x  +  *  a^y  +  a^  (32) 

The  values  of  the  constants  of  Equation  32  are  arrived  at 
based  on  theory,  physical  laws,  reasonable  assumptions,  or 
design  parameters  and  assumed  to  be  greater  than  zero  for  this 
example.  It  is  desired  to  know  the  actual  values  for  these 
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constants.  For  application  of  ZXSSQ,  the  problem  can  be 
formulated  in  the  following  manner. 

Measure  g(x,y)  for  m  known  values  (x,y).  Denote  these 
measured  g(x,y)  as  hj,  h^,  •  •  •  ,  h,  where  hi  corresponds  to 
(Xi,yi).  Compute  gi(Xi,yi)  from  Equation  32  and  then  let 

=  gi  ~  hj.  In  this  problem,  the  five  ai  are  the  variables. 
The  problem  statement  is  then 

-  Minimize:  [fi(x)]^ 

-  Over:  x  =  [  ai,  a^,,  a^,  a,,  a^  ] 

The  problem  is  now  in  a  form  suitable  for  application  of 
ZXSSQ.  The  calling  program  would  provide  the  assumed  values  of 
the  constants  a^,  a^,  aj,  a<  and  a^  as  initial  value  x”.  The 
user  supplied  function  would  compute  values  of  based  on 
(Xi/Vi)/  which  are  fixed  for  a  given  i,  and  x"*\  the  updated  or 
perturbed  values  of  a^,  a^,  aj,  a,  and  a^.  Updated  values  of  fj 
are  then  calculated  using  the  new  values  of  gj.  Note  that  this 
problem  consists  of  5  unknowns,  the  coefficients  of  g  in 
Equation  32,  and  each  measurement  provides  one  known  value  hj . 
Assuming  that  the  actual  system  does  satisfy  a  paraboloid 
relationship  and  in  the  absence  of  measurement  noise,  then  at 
least  5  measurements  are  necessary  for  a  solution. 

3.  Optimization 

ZXSSQ  can  be  employed  in  an  optimization  scheme  in 
which  one  or  more  analytic  expressions  are  minimized.  In  this 
case,  the  constants  are  fixed  and  the  variables  are  perturbed. 
A  simple  example  follows. 
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Clearly,  a  simple  analytic  solution  can  be  obtained 
for  the  minimum  of  the  paraboloid  of  the  previous  example. 
However,  for  illustrative  purposes,  consider  g  to  be  described 
by  two  functions  g^  and  g^  as  shown  in  Equations  33. 

gix.y)  =  g  (x,y)  +  g.  (A',y), 
wheze  g^iXiy)  =  ^5  (33) 

g^  (x,y)  =  a.x  +  a^y 

In  this  example,  the  problem  can  be  stated, 

-  Minimize:  (gi(x))^  +  (g^Cx))^ 

-  Over:  x  =  [  x,  y  ] 

which  is  now  in  the  proper  form  for  application  of  ZXSSQ.  In 
this  case,  the  calling  program  must  supply  an  initial  guess  of 
the  vector  .  If  the  minimized  functions  are  unimodal ,  any 
reasonable  value  of  x°  should  allow  convergence.  Problems 
associated  with  non-unique  solutions  can  be  addressed  in  a 
number  of  different  ways  and  are  somewhat  problem  dependant. 
A  good  initial  estimate  of  x°  when  known  may  suffice. 

Another  problem  which  may  arise  in  an  optimization 
problem  is  that  of  proportionality.  It  is  clear  that  it  would 
suffice  to  minimize  g(x)  rather  than  the  two  functions  g^  and 
g^  although  both  approaches  should  have  similar  results.  The 
particular  problem  formulation  chosen  in  this  case 
demonstrates  two  ways  in  which  a  problem  can  be 
disproportional .  First  of  all,  for  values  of  x  and  y  much 
greater  than  or  less  than  one,  small  changes  in  x  and  y  may 
have  much  greater  effect  on  g^  than  on  gj.  Secondly,  if  as  is 
much  greater  than  zero,  then  at  or  near  the  minimum,  g^  may  be 
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much  greater  than  g^.  Disproportionate  problem  formulation  can 
lead  to  numerical  instability  or  inaccuracies.  Scaling 
techniques,  such  as  dividing  g^  by  a^,  or  reformulating  the 
problem  statement  can  reduce  or  eximinate  difficulties 
associated  with  proportionality. 

C.  KINEMATIC  MODEL  PARAMETER  IDENTIFICATION  METHODOLOGY 
1 .  General  Scheme 

Without  loss  of  generality,  the  general  scheme  will  be 
described  considering  the  previously  described  30  parameter 
PUMA  kinematic  model  and  a  measurement  system  capable  of  full 
pose  measurement.  Less  capable  measurement  systems  generally 
result  in  a  reduction  in  the  number  of  identifiable  parameters 
in  the  model.  However,  the  general  scheme  remains  the  same  and 
the  specific  differences  will  be  addressed  on  a  case  by  case 
basis . 

Given  the  30  parameter  kinematic  model  based  on 
nominal  values,  actual  parameter  identification  or  calibration 
is  performed  in  the  following  manner.  Measurement  of  the  end 
effector  pose  is  made  and  the  joint  variable  values  and  the 
measurement  pose  data  are  both  recorded.  The  manipulator 
joints  are  varied,  additional  measurements  made  and  recorded, 
and  the  process  repeated  until  a  sufficient  data  base  is 
collected.  Sufficient  has  at  least  two  meanings  in  this  case. 
First  of  all,  note  that  the  model  must  reflect  the 
capabilities  of  the  measurement  system.  If  the  measurement 
system  is  capable  of  measuring  full  pose  as  in  this  case,  each 
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measurement  consists  of  six  knowns,  three  positions  and  three 
orientations.  For  an  N  parameter  model,  a  minimum  of  N/6 
measurements  are  required.  However,  some  measurement  noise  is 
inevitable  and  some  larger  number  of  measurements  must  be 
taken  to  achieve  a  desired  accuracy.  Some  additional  factors, 
some  of  which  will  be  discussed  later  and  some  which  are 
issues  for  further  research,  must  be  considered  when 
attempting  to  quantify  the  meaning  of  a  sufficient  data  base. 

The  pose  data  can  be  recorded  in  matrix  form  and  will 
be  denoted  where  E  refers  to  end  effector  as  before,  A 

refers  to  actual  or  measured  and  i  used  to  denote  a  specific 
measurement.  The  forward  kinematic  solution  can  be  computed 
based  on  the  nominal  parameters  and  the  i*^  set  of  joint  angles 
and  stored  in  where  C  refers  to  the  calculated  value. 

Recall  that  when  calculating  the  forward  solution,  both  the 
joint  variable  and  joint  parameter  must  be  taken  into  account. 
A  matrix  ATj  can  be  computed  from  the  difference  of  and 

.  As  described  by  Paul  [Ref.  9],  a  differential 
transformation  matrix  has  the  following  form 

[0-5  6  d  1 

z  y  X 

°  (34) 

-by  b^  0  d, 

0  0  0  0. 

The  A  matrix  of  Equation  34  is  a  good  approximation  for  small 
joint  variable  variations  and  provides  a  reasonable 
approximation  if  all  nominal  parameters  are  "close"  to  their 
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associated  actual  values.  AT^  will  not  necessarily  have  the 
odd  symmetry  of  the  upper  3  by  3  submatrix  of  the  A  matrix  of 
Equation  34.  However,  an  average  value  of  the  magnitude  of  6^, 
and  6,  can  be  computed  as  shown  in  Equation  35 


K  = 


6  ,  = 


t,-,  -  t,. 

2 

2 

2 


(35) 


where  tj,  is  the  i*^^  row  and  j’'*'  column  entry  of  AT^. 

This  problem  can  now  be  stated  in  a  form  acceptable  for 
implementation  of  ZXSSQ: 

Minimize:  (fsjix))'^  .3gx 

i=i  j=i 

Over:  x=  ,6-,  Zf.] 

where  x  is  vector  of  length  30  containing  the  kinematic 
parameters,  N  is  the  number  of  measurements,  i  corresponds  to 
each  of  the  N  measurements  and 


fi 

,1 

=  6x 

f.- 

A 

=  5, 

fi 

,3 

-  s. 

f. 

,4 

f, 

,5  (*) 

=  Cfv 

= 

(37) 


Note  that  6,,  6y,  6,  are  the  average  values  of  ATj  as  computed 
in  Equations  35  and  d. ,  d,,  and  d,  are  the  t^,,  t^^  and  tj, 
elements  of  AT^. 
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2.  Program  ID6  (Generic  Version) 

The  proc,ran  ID6  was  used  to  compute  the  numerical 
solution  of  the  calibration  problem.  A  flowchart  for  the 
program  is  shown  in  Figure  18.  The  program  is  essentially  the 
same  for  all  three  experiments  performed  in  this  thesis. 
Specific  differences  will  be  addressed  as  each  experiment  is 


presented. 


Figure  18.  Program  ID6  Flowchart 
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The  program  reads  an  input  file  consisting  of  the 
kinematic  parameters  for  the  particular  model.  These 
parameters  are  used  to  initialize  the  vector  x  for  the 
subroutine  ZXSSQ.  The  measurement  data  set  is  read  from  a  file 
and  assigned  to  several  different  vectors.  One  vector  contains 
the  full  or  partial  pose  data  of  the  measurements  and  is  of 
length  N  where  N  is  the  number  of  measurements.  In  the  case  of 
full  pose  measurement,  this  vector  has  dimension  4  by  4  by  N 
and  contains  N  T^  matrices.  The  joint  variable  data  is  stored 
in  six  1  by  N  vectors,  one  vector  for  each  joint  variable. 
The  various  parameters  and  options  associated  with  ZXSSQ  are 
initialized  and  then  the  subroutine  is  called. 

Upon  ZXSSQ  termination,  the  updated  values  of  the 
kinematic  parameters  are  output  to  a  file  along  with  the 
residual  values  of  position  and  orientation  as  appropriate. 

Associated  with  each  version  of  ID6  is  the  "user 
defined  subroutine"  called  by  ZXSSQ  titled  PUMA_ARM.  The 
subroutine  is  passed  the  current  value  of  x  for  either 
gradient  estimation  or  computing  the  updated  value  of  the 
minimizing  functions.  The  joint  variable  data  and  measurement 
data  are  passed  via  a  common  block.  The  parameters  are  updated 
by  this  current  value  of  x.  N  iterations  of  the  following 
calculations  are  performed  where  N  is  the  number  of 
measurements . 

During  the  i'"'’  iteration,  the  joint  variables  for 
measurement  i  are  added  to  their  corresponding  joint 
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parameters.  The  forward  kinematic  solution  is  calculated  based 
on  the  current  value  of  the  parameters.  The  minimizing 
functions  for  measurement  i  are  calculated  based  on  the 
forward  solution  and  the  measurement  data  as  described  in  the 
preceeding  section.  The  process  is  repeated  until  i  =  N  at 
which  time  the  function  values  are  passed  back  to  ZXSSQ. 
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III.  THREE  CALIBRATION  TECHNIQUES 


A.  COORDINATE  MEASURING  MACHINE  FULL  POSE  CALIBRATION 
1 .  Physical  Description  of  the  Measurement  System 
a.  The  Coordinate  Measuring  Machine 

The  Coordinate  Measuring  Machine,  CMM,  is 
illustrated  in  Figure  19.  The  horizontal  base  assembly 
consists  of  a  fixed  base  and  a  carriage  which  is  free  to  move 
along  the  length  of  the  assembly.  This  direction  is  usually 
defined  as  the  x  axis.  The  carriage  is  held  in  alignment  by 
two  guide  bars  which  have  precision  racks  machined  on  their 
surfaces.  The  racks  provide  motion  through  a  rack  and  pinion 
arrangement  and  rotation  of  the  x  axis  knob  as  shown  in  Figure 
19.  Optical  encoders  in  the  carriage  assembly  provide 
displacement  measurements.  The  vertical  column  is  constructed 
and  functions  in  a  manner  similar  to  the  x  axis  carriage.  This 
direction  is  designated  as  the  y  axis.  Motion  in  the  z 
direction  is  accommodated  by  the  horizontal  assembly  mounted 
on  the  y  axis  carriage  as  shown,  and  is  constructed  and 
functions  in  the  same  manner  as  the  other  two  axes. 

A  display  unit,  not  shown,  is  provided  and  is 
capable  of  indicating  either  in  inches  or  millimeters.  The  CMM 
is  capable  of  0.01  mm  accuracy  on  all  axis.  The  display  output 
can  be  zeroed  for  all  axes  simultaneously  by  depressing  the 
"ALL  ZERO"  pushbutton  or  each  axis  can  be  zeroed  separately  by 
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pressing  the  appropriate  axis  “zero"  pushbutton  and  contact 
with  a  touch  probe. 


Figure  19.  The  Coordinate  Measuring  Machine  (CMM) 

The  touch  probe,  illustrated  in  Figure  20,  is 


mounted  on  the  end  of  the  z  axis  of  the  CMM.  The  touch  probe 
tip  is  a  machined  sphere  of  3.0  mm  diameter.  When  the  probe 
comes  in  contact  with  an  object,  the  indicator  will  illuminate 
and  the  display  unit  readout  will  hold  its  present  reading 
until  the  probe  is  no  longer  in  contact.  When  the  display  unit 
"zero"  is  set  for  a  particular  axis,  the  axis  readout  is 
zeroed  by  touch  probe  contact.  This  is  a  useful  feature 


48 


Figure  20.  CMM  Touch  Probe 


because  it  provides  a  method  of  establishing  a  reference  frame 
external  to  both  the  manipulator  and  the  CMM,  which  will  be 
described  in  the  following  sections. 

A  machined  cube,  similar  to  that  illustrated  in 
Figure  21,  was  mounted  in  the  common  working  volume  of  the 
PUMA  and  the  CMM.  The  faces  of  the  cube  were  nominally  aligned 
with  the  axis  of  the  CMM.  A  corner  of  the  cube  was  chosen  as 
the  reference  point  of  the  measurement  system  which  eliminated 
the  need  for  absolute  alignment  of  the  cube  faces  with 
parallel  planes  formed  by  the  CMM  x,  y  and  z  axis.  Using  the 
touch  probe  zero  reference  feature,  a  reference  point  could  be 
established.  For  example,  with  the  display  unit  'x-zero' 
enabled,  the  probe  can  be  placed  near  the  y-z  face  of  the  cube 
in  close  proximity  to  the  reference  corner  and  then  slowly 
moved  in  the  x  direction  until  contact  is  made  and  the  x=0 
reference  established. 
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Figure  21.  Measurement  System  Reference  Cube 

b.  Manipulator  End  Effector 

To  calibrate  a  30  parameter  model  of  the  PUMA  560 
requires  measurement  of  the  pose  of  the  end  effector.  Since 
the  CMM  is  only  capable  of  position  measurement,  the  end 
effector  must  be  of  some  known  geometry  such  that  orientation 
can  be  calculated  from  a  series  of  position  measurements.  The 
end  effector  illustrated  in  Figure  22  was  used  in  this 
experiment . 

The  five  machined  tooling  balls  of  radius  6.35  mm 
are  mounted  orthogonally  to  the  circular  plate  and  post  as 
shown.  The  fabrication  process  guaranteed  orthogonality  of  the 
fixture,  but  the  specific  location  of  each  tooling  ball  on  its 
respective  axis  was  not  guaranteed.  These  positions  were 
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Figure  22.  End  Effector  for  Full  Pose  Measurement 

determined  by  a  statis  calibration  with  the  CMM.  The  balls 
were  numbered  one  through  five  and  their  corresponding  axis 
and  distance  along  the  axis  to  the  origin  of  the  coordinate 
frame  are  listed  in  Table  2.  The  lower  flange  is  mated  with 
the  PUMA  end  effector  mounting  flange. 

2 .  Theory 

a.  Closed  Chain  Kinematic  Model 

The  link  parameter  table  listed  previously  in 
Table  1,  is  used  for  manipulator  transformations  in  all  three 
experiments.  The  only  difference  in  the  models  for  each 
experiment  is  in  the  identifiable  parameters  of  T„°  and 
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TABLE  2.  END  EFFECTOR  BALL  DESIGNATIONS  AND  DIMENSIONS 


Ball  # 

Axis 

Distance  (mm) 

1 

z 

51.111 

2 

X 

50.740 

3 

y 

50.703 

4 

-X 

50.913 

5 

-y 

50.988 

Since  the  CMM  is  capable  of  fully  defining  its  own  coordinate 
system,  and  with  the  use  of  the  previously  described  end 
effector,  full  pose  measurement  is  possible  and  all  30 
parameters  of  the  model  can  be  identified.  Table  3  lists  the 
nominal  values  for  the  12  parameters  of  T„°  and  T5®  and  are 
based  on  the  nominal  position  of  the  measurement  system 
reference  point,  orientation  of  the  CMM  with  respect  to  the 
PUMA  base  frame  and  the  nominal  orientation  of  the  end 
effector.  The  values  are  typed  in  bold  to  emphasize  that  they 
are  all  identifiable. 

b.  Developing  Full  Pose  Data 

To  determine  the  pose  of  the  coordinate  frame 
defined  on  the  end  effector  first  requires  knowledge  of  the 
coordinates  of  the  center  of  the  tooling  balls  with  respect  to 
F".  Recall  the  relationship  given  in  Equation  1  between  the 
radius  r  of  a  sphere,  its  center  at  Xo,  yo,  and  Zo,  and  the 
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TABLE  3.  KINEMATIC  PARAMETER  TABLE  FOR  T„°  AND  T^' 


m  o 

T." 

0 

180.0* 

0 

o 

• 

o 

a\ 

e 

0 

o 

• 

o 

o 

• 

o 

0 

'I' 

90.0* 

0 

o 

• 

o 

X 

-394.0  mm 

0.0  mm 

y 

-383.0  mm 

0.0  mm 

z 

474.0  mm 

134.0  mm 

coordinates  of  a  point  P  on  the  surface  of  the  sphere. 

r  =  +  (yo-Py)^  +  (Zq-Pz^' 

Since  measurement  of  points  on  the  surface  of  the  sphere  are 
possible  with  the  CMM  and  the  radius  of  the  precision  tooling 
balls  is  known,  then  three  unknowns  remain  in  Equation  1.  This 
implies  that  a  minimum  of  three  measurements  are  required  for 
a  fully  defined  problem  involving  three  non-linear  equations 
with  three  unknowns.  ZXSSQ  can  be  employed  for  a  solution  with 
the  following  problem  statement: 

-  Minimize:  E  (fi(x))^ 

-  Over:  x  =  [  Xq,  Yo,  Zo  ] 
where 

-  Xo,  yo  and  Zo  are  the  coordinates  of  the  center  of  the 
sphere 
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-  fi(x)  =  1  r  -  Ti  I 

-  r  is  the  radius  of  the  sphere 

-  Tj,  is  calculated  from  Equation  1  for  the  i‘^  measurement  of 
a  point  on  the  tooling  ball 

Simulation  and  experiment  showed  that  four  measurements 
provided  sufficient  accuracy. 

With  the  coordinates  of  the  center  of  the  tooling 
balls  now  available,  determination  of  the  pose  can  be 
developed.  Recall  that  the  coordinates  of  a  point  P  described 
with  respect  to  can  be  transformed  into  F”  coordinates  by 

Py_  =  Tm  Pe 

Unfortunately,  in  this  case,  P„  and  Pj.  are  known  and  is  the 
unknown  and  the  vector  of  the  coordinates  of  P^  can  not  be 
inverted  in  order  to  solve  for  the  transformation.  However,  as 
described  in  Paul  [Ref.  10],  all  the  coordinates  of  an  object 
can  be  transformed  from  one  frame  to  another  simultaneously  by 
composing  a  matrix  whose  columns  are  the  coordinates  of  the 
object  to  be  transformed  and  then  pre-multiplying  by  the 
transformation  matrix  describing  the  frame.  Note  that  the 
points  are  described  by  the  usual  augmented  4x1  vectors.  Let 
P„  and  Pi  denote  two  matrices  composed  of  the  coordinates  of 
the  center  of  four  balls,  then 

Pm  =  P, 

Pm  Pm  = 

It  may  not  always  be  possible  to  measure  four 
balls  for  any  given  pose.  However,  with  the  given  geometry,  a 
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fourth  location,  orthogonal  to  three  measured  balls,  can  be 
synthesized  by 

P,  =  (  P,  -  Pi  )  X  (  P,  -  Pi  )  (41) 

where  the  subscripts  are  not  intended  to  imply  any  order  or 
particular  ball,  but  only  three  different  balls.  This 
calculation  must  be  performed  twice,  once  for  points  described 
with  respect  to  F”  and  once  for  points  described  with  respect 
to  F\ 

3.  Simulation 

a.  Introduction 

The  calibration  process  is  well  suited  to  computer 
simulation  for  the  following  reasons: 

-  Experimental  data  simulation,  including  noise  injection, 
is  usually  a  straight  forward  process. 

-  The  heart  of  the  process  is  a  numerical  solution  performed 
by  computer. 

-  Analysis  of  the  results  is  easily  performed  on  computer. 


Several  advantages  are  offered  by  first  performing  a  computer 
simulation : 

-  The  identification  algorithm  can  be  tested. 

-  Trends  in  the  accuracy  of  the  solution  when  compared  with 
the  number  of  measurements  based  on  predicted  noise  level 
can  be  identified. 

-  To  some  extent,  the  model  can  be  validated  during  the 
simulation.  For  example,  if  dependant  parameters  are 
included  in  the  model,  the  identification  algorithm  will 
not  converge  to  the  correct  solution  since  no  unique 
solution  exists. 
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The  general  simulation  scheme  used  tor  this 
experiment  is  illustrated  in  Figure  23.  The  programs  and  their 
associated  input  and  output  files  are  described  in  the 


following  sections. 


Figure  23.  CMM  Simulation  Scheme 
b.  The  Program  JOINT 

The  program  joint  uses  a  Monte-Carlo  method  random 
number  generator  to  produce  random  sets  of  six  joint  variable 
values.  Three  options  for  the  range  of  joint  motion  for  each 
joint  are  available.  Full  range,  one-half  of  normal  range,  and 
one-guarter  of  normal  range  of  joint  motion  can  be  selected 
and  this  allows  simulation  of  the  effects  of  a  limited  working 
volume.  The  number  of  sets  of  joint  angles  is  interactively 
supplied  to  the  program.  The  program  writes  the  joint  angle 


sets  to  a  file  titled  PUMA_VAR.DAT.  The  program  is  executed 
twice  with  the  second  output  file  renamed  POSVER.DAT  for  use 
in  a  verification  program  which  will  be  described  later. 
c.  The  Program  POSE 

Program  POSE  reads  a  file,  INPUT.DAT,  containing 
the  following  information: 

-  the  previously  described  kinematic  parameter  table; 

-  the  number  of  observations  or  measurements  simulated; 

-  the  number  of  model  parameters; 

-  length  and  angular  offsets; 

-  length  and  angular  noise  scaling  constants. 

The  length  and  angular  offsets  are  added  to  each  of  the 
identifiable  parameters  so  that  a  known  model,  different  from 
the  nominal  model,  can  be  used  to  generate  the  simulated  pose 
measurements.  POSE  reads  the  random  sets  of  joint  angles  from 
the  file  PUMA_VAR.DAT.  The  joint  variable  angles  are  added  to 
the  offset  joint  parameters  and  a  forward  kinematic  solution 
is  calculated.  The  resulting  solution,  a  4x4  matrix  based 
on  offset  parameters,  is  stored  in  a  file  titled  PUMA_POS.DAT 
along  with  the  corresponding  set  of  joint  variable  data.  Prior 
to  storage,  noise  can  be  injected  into  the  joint  variable 
angles,  and  separately  into  the  orientation  and  position 
elements  of  T*^.  The  random  noise  is  calculated  by  scaling  the 
output  from  a  Monte-Carlo  random  number  generator  and  then 
added  to  the  desired  parameters. 
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d.  The  Program  CID6 


The  previously  described  file  INPUT.DAT  is  read 
and  stored  in  CID6.  The  identifiable  nominal  parameters  are 
used  to  initialize  the  ZXSSQ  x  vector.  The  simulated  pose 
measurements  and  joint  variable  data  are  read  from 
PUMA_POS.DAT  and  stored  in  their  respective  arrays.  ZXSSQ 
parameters  are  initialized  and  then  the  subroutine  is  called. 

ZXSSQ  and  the  external  subroutine  PUMA_ARM 
perform  the  identification  process  as  described  in  the 
Kinematic  Model  Identification  Methodology  .ection  general 
scheme,  and  summarized  in  Equation  sets  36  and  37. 

After  termination  of  ZXSSQ,  the  calibrated  link 
parameter  table  is  written  to  a  file  titled  RESULT.DAT.  Up  to 
this  point,  the  simulation  version  of  this  program  is 
identical  to  the  experimental  version.  However,  in  the 
simulation,  the  actual  model  parameters  are  known  since  they 
were  formed  by  adding  a  known  offset  to  the  nominal 
parameters.  Therefore,  the  accuracy  of  the  calibrated 
parameters  can  be  determined  by  again  adding  the  offset  values 
to  the  nominal  parameters  and  comparing  these  values  to  the 
calibrated  values.  An  rms  value  for  both  length  and  angular 
parameters  is  computed  and  also  written  to  RESULT.DAT  along 
with  the  residual  from  ZXSSQ. 

e.  The  Program  VERIFY 

As  a  final  test  of  the  accuracy  of  this  method, 
the  program  VERIFY  computes  a  forward  solution  based  on  the 
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actual  model  parameters  and  the  calibrated  parameters,  and 
compares  the  resulting  poses.  To  compute  the  actual  pose, 
VERIFY  reads  file  INPUT.DAT  and,  as  before,  adds  the  length 
and  angular  offsets  to  the  appropriate  kinematic  parameters. 
The  calibrated  kinematic  parameter  table  is  read  from  the  file 
RESULT.DAT.  Two  separate  forward  solutions  are  calculated 
based  on  the  two  sets  of  kinematic  parameters  and  sets  of 
random  joint  angles  produced  by  JOINT  and  read  from  the  file 
POSVER.DAT  described  earlier.  An  average  orientation  and 
position  error  is  calculated  from  the  difference  between  the 
two  T'^  matrices  resulting  from  the  forward  solution 
calculations . 

4 .  Experiment 

a.  Data  Acquisition 

As  noted  in  a  preceding  section  on  developing  full 
pose  data,  four  measurements  with  the  CMM  are  required  to 
locate  the  center  of  one  tooling  ball  of  the  end  effector. 
Additionally,  the  centers  of  three  tooling  balls  were  required 
to  develop  a  full  pose  measurement  T*^.  A  program,  CMMPOSE,  was 
written  to  convert  the  CMM  position  measurements  into  pose 
measurements . 

CMMPOSE  is  given  three  sets  of  four  CMM  position 
measurements  interactively  along  with  the  associated  ball 
numbers.  A  subroutine,  BALL,  calculates  the  center  of  each 
tooling  ball  using  the  previously  described  algorithm.  After 
the  center  of  each  ball  is  calculated,  the  residual  is 
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displayed  on  the  terminal  and  the  user  is  prompted  to  either 
keep  or  reject  the  value  based  on  the  residual.  Residuals 
greater  than  10'*  were  rejected  and  a  second  set  of 
measurements  taken.  This  process  allows  the  user  to  reject 
poor  data,  probably  caused  by  incorrect  reading  of  the  CMM 
display  or  data  entry  errors.  CMMPOSE  then  synthesizes  a 
fourth  spatial  position,  composes  the  two  position  matrices 
described  earlier  and  then  computes  the  measured  pose.  An 
orthogonality  check  is  performed  by 

n  o,  a  •  a,  o  •  a  (42) 

where  n,  o  and  a  are  vectors  corresponding  to  the  first  three 
columns  of  T*  and  is  the  dot  product  operator  as  before. 
Since  these  vectors  correspond  to  rotated  coordinate  axes  then 
their  dot  products  should  be  zero.  If  the  orthogonality  check 
passes,  then  the  program  stores  the  matrix  and  the  current 
joint  angle  variables  which  are  interactively  input  by  the 
user  in  a  file  PUMA_POS.DAT.  Joint  angle  variables  of  the  PUMA 
560  can  be  obtained  from  the  PUMA  560  console  by  typing 
"where"  on  the  keyboard. 

Data  was  acquired  by  first  zeroing  the  CMM  at  the 
reference  point  as  described  earlier.  The  CMM  was  slowly 
positioned  so  that  light  contact  was  made  with  the  surface  of 
one  of  the  tooling  balls.  Care  was  exercised  to  insure  that 
the  direction  of  approach  was  nearly  normal  to  the  tangent 
planes  of  the  tooling  ball  and  to  the  touch  probe  tip,  at 
their  contact  point.  This  reduces  possible  errors  caused  by 
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deflection  of  the  tip  away  from  the  point  of  contact.  This 
process  is  repeated  with  the  position  data  supplied  to 
CMMPOSE,  along  with  the  corresponding  joint  variable  angles. 
Two  operators,  one  operating  the  CMM  and  positioning  the  PUMA 
with  the  teach  pendant,^  and  another  entering  data  at  a 
console  can  expect  to  make  one  full  pose  measurement  in 
approximately  10  minutes.  Figure  24  illustrates  the  CMM  ready 
for  a  position  measurement  to  be  taken. 

Jb.  Parameter  Identification  and  Verification 

Program  CID6  was  modified  as  described  earlier  for 
experimental  data.  A  total  of  44  poses  were  collected  and  the 
entire  set  of  poses  were  used  to  calibrate  the  PUMA.  Table  4 
lists  the  nominal  and  calibrated  values  of  the  kinematic 
parameters.  Length  and  angular  parameters  are  reports  in  units 
of  millimeters  and  degrees,  respectively. 

Since  the  actual  parameters  are  not  known  as  they 
are  in  the  simulation,  a  program  such  as  VERIFY  cannot  be 
used.  However,  to  better  evaluate  the  accuracy  of  the 
resulting  calibration,  the  set  of  44  poses  and  associated 
joint  variable  angles  were  divided  equally  into  two  sets, 
and  Sj.  Si  was  used  to  perform  a  calibration.  Sj  was  then  used 
in  a  verification  process  in  the  following  manner.  Forward 


^The  PUMA  560  can  be  controlled  by  computer  or  manually 
controlled  by  the  teach  pendant.  The  teach  pendant  allows  an 
operator  individual  positioning  of  each  joint  when  operated  in 
•’joint  mode".  Additional  teach  pendant  modes  are  available  for 
positioning  the  end  effector  with  respect  to  the  manipulator  tool 
frame  or  base  frame. 
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Figure  24.  Full  Pose  Measurement  With  the  CMM 

solutions  using  the  joint  variable  angles  of  and  the 
parameters  calibrated  with  were  calculated.  The  difference 
between  these  calculated  poses  and  the  poses  of  provides  a 
good  indication  of  the  improvement  in  accuracy.  An  rms  value 
of  0.3  mm  was  calculated  for  the  position  error. 
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measurement  system  and  the  modification  will  be  described  in 
the  following  sections.  Although  the  method  offered  several 
apparent  advantages  over  the  CMM  calibration  method,  the 
resulting  accuracy  was  less  by  a  factor  of  three.  Two  possible 
explanations  for  the  resulting  loss  of  accuracy  are  increased 
measurement  noise  due  to  loading  effects  and  limited  range  of 
joint  rotation.  The  modification  described  in  the  following 
was  attempted  to  improve  the  range  of  joint  motion  and  thereby 
increase  the  overall  calibration  accuracy. 

2.  Physical  Description  of  the  Measurement  System 

In  the  original  experiment,  the  vertical  post  (y  and 
z  axis)  of  the  CMM  were  removed.  A  plate  was  manufactured 
which  mates  flush  with  the  end  effector  flange  of  the  PUMA  and 
flush  with  the  X  carriage  of  the  CMM  in  the  position  vacated 
by  the  vertical  post.  In  this  configuration,  the  orientation 
of  F'^  is  fixed  and  the  end  effector  restricted  to  linear 
motion  along  the  x  axis  of  the  CMM  base.  The  experimental 
setup  is  illustrated  in  Figure  25.  As  shown  in  the  figure,  the 
CMM  base  was  placed  on  a  ramp.  The  ramp  was  necessary  to 
ensure  adequate  rotation  of  all  PUMA  joints.  For  example,  due 
to  the  design  of  the  PUMA,  if  the  slide  was  placed 
horizontally  on  the  table,  essentially  no  rotation  of  the 
wrist,  joint  4,  occurs  during  translation  along  the  axis  of 
the  CMM  base.  With  no  wrist  rotation,  the  PUMA  becomes  a  five 
degree  of  freedom  manipulator  and  the  kinematic  model  must  be 
changed  to  reflect  this  apparent  loss  of  a  joint.  Additional 
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problems  may  be  encountered  with  respect  to  identification 
when  joint  rotation  is  limited  and  these  problems  will  be 
discussed  in  greater  detail  later.  The  position  and 
orientation  of  the  ramp,  as  shown,  offered  reasonable  joint 
excursion  for  all  joints  as  the  end  effector  moved  along  the 
slide . 


Figure  25.  PUMA  Calibration  with  a  Linear  Slide 
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To  improve  the  range  of  joint  rotation,  a  ball  joint 
was  placed  between  the  PUMA  end  effector  flange  and  the  X 
carriage  of  the  CMM.  A  drawing  of  the  fixture  can  be  seen  in 
Figure  26.  In  this  configuration,  the  complete  range  of  motion 
of  joints  four,  five  and  six  could  be  achieved.  Approximately 
20  additional  degrees  of  rotation  could  be  achieved  for  joints 
two  and  three.  However,  joint  one  rotation  remained 
essentially  unchanged. 

Note  that  for  both  methods,  the  end  effector  of  the 
PUMA  is  physically  constrained,  which  requires  the  PUMA  to  be 
placed  in  "free”  mode.^  Consequently,  the  PUMA  must  be 
supported  by  an  operator  during  calibration  to  prevent  it  from 
collapsing  and  damaging  itself  or  the  measurement  system. 

3.  Closed  Chain  Kinematic  Model 

The  closed  chain  kinematic  model  for  the  modified 
linear  slide  method  varies  significantly  from  the  original 
linear  slide  method,  which  will  be  described  in  another 
section  of  this  thesis.  As  noted  previously,  the  kinematic 
parameters  listed  in  Table  1  are  used  to  describe  the 
manipulator.  It  is  the  parameters  of  T„°  and  that  must  be 
determined.  First,  consider  the  ball  joint  attached  to  the  end 
effector  flange.  A  ball  joint  has  three  degrees  of  freedom. 


^Each  joint  of  the  PUMA  is  equipped  with  a  coarse  and  fine 
joint  encoder  and  servomotor.  Additionally,  joints  one,  two  and 
three  are  equipped  with  brakes  which  prevent  the  manipulator  from 
collapsing  when  power  is  removed  from  the  servomotors.  In  "free” 
mode,  power  is  supplied  to  the  joint  encoders  but  no  power  is 
supplied  to  the  joint  motors  and  all  brakes  are  released. 


66 


Figure  26.  Modified  Linear  Slide  End  Effector 


rotation  about  any  set  of  three  orthogonal  axes  placed  at  the 
center  of  the  ball.  Ball  joints  are  fixed  in  translation. 
Arbitrary  rotational  motion  and  the  fixed  translation 
relationship  as  observed  through  a  series  of  measurements  will 
define  a  point  in  space,  but  no  set  of  fixed  axes  can  be 
associated  with  this  point.  Therefore,  is  assigned  to  the 
point  in  space  defined  by  the  ball  joint.  Note  that  the  ball 
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joint  must  be  "exercised"  in  all  its  degrees  of  freedom  as 
observed  through  a  series  of  measurements  in  order  for  the 
model  to  hold.  Now  as  the  point  defined  by  the  ball  joint 
translates  along  the  axis  of  the  CMM  base,  a  specific  axis 
rather  than  a  direction  is  defined.  The  location  of  F”  is 
arbitrarily  defined  by  zeroing  the  readout  on  the  CMM  display 
unit  at  some  point  along  the  axis.  This  zero  point  is  fixed  in 
space  with  respect  to  the  ball  joint  location.  F”  is  then 
defined  to  be  this  fixed  point  in  space  and  the  axis  along 
which  the  ball  joint  travels  is  associated  with  F”  as  well. 
Summarizing,  F”  consists  of  a  fixed  point  and  an  axis,  and  F'^ 
consists  of  simply  a  point.  As  noted  earlier,  any  three  of  the 
six  parameters  of  RPYT  are  sufficient  to  transform  from  a 
frame  to  a  point  as  long  as  one  of  the  parameters  is  a 
translation.  The  rotation  Rot(0,Z6)  and  translations  along  x 
and  z  were  chosen  for  T^*^.  Since  F"  is  not  fully  defined,  the 
transformation  T„°  is  developed  by  considering  To”  and  then 
inverting  this  transformation.  In  general,  two  rotations  are 
required  to  align  a  particular  frame  axis  with  an  arbitrary 
axis  in  space.  Three  more  parameters  are  then  required  to 
transform  to  a  point  on  the  axis.  The  kinematic  parameters  for 
T„°  and  T5*  are  listed  in  Table  5  where  the  identifiable 
parameters  are  typed  in  bold. 

With  these  eight  identifiable  parameters  added  to  the 
18  identifiable  parameters  of  the  PUMA  kinematic  model,  the 
closed  chain  model  consists  of  a  total  of  26  identifiable 
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TABLE  5.  KINEMATIC  PARAMETERS  FOR  T„°  AND  T^' 


rp  o 
^  M 

0 

195.0* 

-41.0° 

e 

-30.0° 

0.0° 

o 

• 

o 

o 

o 

X 

-180.0  mm 

78.0  mm 

y 

-380.0  mm 

0 . 0  mm 

z 

360.0  mm 

79.0  mm 

parameters.  With  the  PUMA  end  effector  ball  joint  at  some 
position  along  the  slide,  the  x,  y  and  z  position  is  known 
with  respect  to  F”.  The  x  coordinate  is  the  displacement  of 
the  ball  joint  from  the  zero  reference,  and  the  y  and  z  values 
are  zero.  Th-^refore,  a  minimum  of  9  measurements  are  necessary 
for  a  solution  in  the  absence  of  noise. 

4.  Simulation 

a .  Introduction 

In  the  CMM  experiment,  simulation  data  was  easily 
developed  by  generating  a  random  set  of  joint  variables  and 
computing  the  forward  kinematic  solution  since  the  pose  of  the 
end  effector  was  not  important.  However,  in  this  experiment, 
for  a  given  x  coordinate,  the  end  effector  position  is  known 
and  the  joint  variable  angles  corresponding  to  that  position 
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must  be  determined.  This  is  a  special  case  of  the  so  called 
inverse  kinematic  solution.  In  general,  there  is  no  guarantee 
that  an  analytic  inverse  solution  exists.  Furthermore,  there 
are  problems  of  uniqueness  and  numerical  instabilities  that 
arise  from  the  cosecant  and  secant  functions  that  are  inherent 
in  the  analytic  expressions.  Consequently,  a  numerical 
approach  is  used  in  this  case  to  compute  the  simulation  data. 

The  position  data  can  be  generated  by  randomly 
generating  a  value  for  x  displacement  along  the  slide.  As 
stated  earlier,  the  y  and  2  components  are  zero.  These  values 
become  the  "desired"  end  effector  position.  For  a  given  set  of 
joint  variables,  a  forward  solution  based  on  the  given  model 
can  be  calculated  resulting  in  an  end  effector  position.  The 
difference  between  the  calculated  end  effector  position  and 
the  "desired"  position,  becomes  a  set  of  error  functions  to  be 
minimized.  The  problem  statement  in  a  form  suitable  for  ZXSSQ 
implementation  is: 

-  Minimize:  (fi(x))^  +  (fj(x))^  +  (fjCx))^ 

-  Over:  x  =  [S^,  6,,  B,,  6,,  65,  0*] 

where  f^  is  the  difference  between  the  calculated  and 
"desired"  x  coordinate  of  the  end  effector,  and  fj  and  fj  are 
the  calculated  y  and  z  coordinates. 

The  suite  of  simulation  programs  and  their 
interaction  is  similar  to  the  CMM  simulation  scheme  and  is 
illustrated  in  Figure  27.  Program  listings  arr  found  in 
-ppendices  A,  B  and  C. 
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Figure  27.  Modified  Linear  Slide  Simulation  Scheme 
Jb.  The  Program  BLINSC 

BLINSC  reads  in  the  kinematic  parameter  table  and 
additional  information  as  described  in  the  CMM  experiment  from 
a  file  titled  INPUT.DAT.  Known  length  and  angular  offsets  are 
added  to  the  parameters.  The  x  vector  described  in  the 
preceding  paragraph  is  initialized.  The  x  vector  can  be 
initialized  to  all  zeroes  or  may  be  initialized  such  that 
solutions  are  "driven"  to  a  particular  arm  configuration.^  The 
program  then  generates  a  random  value  of  the  slide  position, 


^Referring  back  to  Figure  16,  the  base  of  the  manipulator  is 
sometimes  referred  to  as  the  waist  and  Joint  1  rotation  would  be  at 
the  waist.  Joint  2  then  corresponds  to  a  shoulder  joint.  Joint  3 
is  referred  to  as  the  elbow  joint.  With  this  terminology,  when 
viewing  the  robot,  four  possible  arm  configurations  are  possible: 
"lefty-elbow  up",  "lefty-elbow  down",  "righty  elbow  up",  and 
"righty-elbow  down".  The  manipulator  is  shown  in  "lefty-elbow  down 
configuration  in  Figure  24. 
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X  coordinate  of  the  end  effector,  and  the  ZXSSQ  option 
parameters  are  assigned  appropriate  values.  ZXSSQ  is  then 
called  to  compute  the  inverse  kinematic  solution  as  described 
in  the  preceding  section.  A  modified  version  of  the  subroutine 
PUMA_ARM  utilized  previously  in  the  program  CID6  is  then 
incorporated  in  the  program  as  the  ZXSSQ  user  supplied 
subroutine  for  calculation  of  the  forward  solution  and 
evaluation  of  the  error  functions.  Upon  termination  of  ZXSSQ, 
the  program  provides  capability  for  noise  injection  on  both 
the  three  coordinates  and  the  joint  variables.  The  simulated 
joint  variables  and  end  effector  coordinates  are  then  written 
to  a  file  titled  PUMA_POS.DAT.  The  preceding  process  is 
repeated  until  the  desired  amount  of  simulated  data  has  been 
generated . 

c.  The  Program  BID6 

The  only  significant  modification  to  previously 
described  versions  of  ID6  is  to  the  ZXSSQ  user  supplied 
subroutine  PUMA_ARM.  Additionally,  modifications  due  to  the 
changes  in  the  number  of  identifiable  parameters  of  the  model, 
such  as  the  size  and  makeup  of  the  x  vector,  are  incorporated 
into  BID6.  The  modification  to  PUMA_ARM  is  similar  to  that 
used  in  BLINSC.  Since  only  the  position  of  is  identifiable, 
only  three  error  functions  can  be  calculated  for  each 
measurement  set  which,  in  this  case,  is  the  measured  x,  y  and 
2  position  and  the  associated  joint  variables  for  that 
position.  The  error  functions  are  the  difference  between  the 
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calculated  position  of  based  on  the  current  value  of  all 
parameters  and  joint  variables  and  the  measured  position. 
After  execution  of  ZXSSQ,  the  calibrated  parameters  are 
written  to  a  file  RESULT.DAT  as  before.  Additionally,  the 
known  position  error  only  is  calculated  and  written  to 
RESULT . DAT . 

d-  The  Program  BVERIFY 

As  a  final  test  of  the  accuracy  of  this  method, 
the  program  VERIFY  described  for  the  CMM  experiment  was 
modified.  BVERIFY  computes  a  forward  solution  based  on  the 
actual  model  parameters  and  the  calibrated  parameters,  and 
compares  the  resulting  poses.  To  compute  the  actual  pose, 
BVERIFY  reads  file  INPUT.DAT  and,  as  before,  adds  the  length 
and  angular  offsets  to  the  appropriate  kinematic  parameters. 
The  calibrated  kinematic  parameter  table  is  read  from  the  file 
RESULT.DAT.  Two  separate  forward  solutions  are  calculated 
based  on  the  two  sets  of  kinematic  parameters  and  sets  of 
random  joint  angles  produced  by  BLINSC  and  read  from  a  renamed 
file  POSVER.DAT.  An  average  position  error  is  calculated  from 
the  difference  between  the  position  entries  of  the  two 
matrices  resulting  from  the  forward  kinematic  solution 
calculations . 

5 .  Experiment 

a.  Data  Acquisition 

With  the  CMM  base  rigidly  affixed  to  the  ramp  and 
the  ramp  secured  to  the  table,  the  upper  ball  joint  flange  is 
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bolted  to  the  PUMA  end  effector  flange  and  the  end  effector 
positioned  above  the  slide  by  use  of  the  teach  pendant.  With 
one  operator  supporting  the  PUMA,  a  second  operator  places  the 
manipulator  in  "free"  mode.  The  manipulator  is  then  maneuvered 
into  position  so  that  the  lower  ball  joint  flange  can  be 
bolted  to  the  X  carriage  of  the  CMM.  Once  the  lower  flange  is 
bolted,  the  carriage  is  placed  at  a  predetermined  location  and 
the  display  unit  readout  zeroed.  This  location  with  respect  to 
approximate  location  of  the  base  or  zero  frame  of  the 
manipulator  is  measured  and  recorded  for  incorporation  into 
the  kinematic  parameter  table  along  with  the  ramp  orientation 
and  the  ball  joint  position  and  orientation  with  respect  to 
frame  5  of  the  manipulator. 

At  the  zero  position  and  nine  other  positions 
located  at  approximately  80  mm  intervals,  four  separate  sets 
of  measurements  were  recorded.  The  position  for  each  set 
remained  fixed  by  tightening  the  provided  thumbscrew  onto  one 
of  the  guide  bars.  The  manipulator  was  then  maneuvered  into 
four  widely  varying  configurations  and  the  joint  variables  for 
each  configuration  recorded.  After  completion  of  the  40 
measurements,  the  end  effector  was  unbolted  and  then  joint  one 
was  rotated  to  the  other  arm  configuration.  A  second  set  of 
measurements  in  the  second  arm  configuration  were  recorded. 

Jb.  Parameter  Identification  and  Verification 

The  entire  set  of  80  measurements  were  then  used 
by  an  experimental  version  of  BID6  for  actual  parameter 


identification.  Table  6  lists  the  nominal  and  calibrated 


parameters  for  this  method.  As  before,  length  and  angular 
parameters  are  reported  in  units  of  milli-meters  and  degrees, 
respectively.  As  in  the  CMM  experiment,  the  data  was  then 
split  into  two  groups  for  verification.  The  resulting 
positional  error  was  0.744  mm. 

TABLE  6.  NOMINAL  AND  CALIBRATED  KINEMATIC  PARAMETERS 


Nominal 

Calibrated 

195.0 

194 . 903 

0H 

-30.0 

-30.887 

-180.0 

-179.567 

Yh 

-380.0 

-378.528 

Zm 

360 . 0 

355.635 

ai 

0.0 

-0.096 

Qi 

-90.0 

-89.823 

56^ 

0.0 

-0.340 

431.85 

431.123 

0.0 

0.580 

0.0 

0 .485 

(503 

0.0 

-0.993 

d. 

149 . 09 

146.028 

93 

-20.33 

-20.255 

0:3 

90.0 

90 .415 

50. 

0.0 

-1.089 

d. 

433.0 

434.095 

a. 

0.0 

0.074 

a* 

-90.0 

-90.244 

605 

0.0 

1.293 

d5 

0.0 

-0.863 

a^ 

0.0 

-0.175 

90.0 

89.905 

-41.0 

-41.355 

Xp 

78.0 

78.404 

Z. 

79.0 

79 . 203 
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C.  THE  WIRE  POTENTIOMETER  METHOD 


1 .  Introduction 

Calibration  of  PUMA  560  was  performed  by  Oriels  [Ref. 
12]  utilizing  an  instrument  referred  to  as  a  ball  bar.  The 
instrument  consisted  of  a  rigid  bar  of  known  fixed  length  with 
a  ball  joint  attached  at  each  end.  One  end  of  the  ball  bar  was 
affixed  to  a  work  surface  within  the  working  volume  of  the 
PUMA.  The  other  ball  joint  was  attached  to  the  end  effector 
flange  of  the  robot.  The  method  offered  several  advantages 
including : 

precise  length  measurements 
essentially  no  measurement  noise 
ease  of  fabrication 
low  cost 

The  major  disadvantages  with  the  method  are: 

Calibration  can  only  be  performed  on  a  manipulator  with 
a  "free"  mode  of  operation. 

The  method  requires  at  least  two  operators  so  that  the 
manipulator  is  supported  while  collecting  data. 

The  end  effector,  and  hence  data  collection,  is  limited 
to  the  surface  of  a  sphere  of  ball  bar  length  radius 
which  may  limit  its  applicability  in  certain 
environments . 

The  wire  potentiometer  method  was  developed  to  retain 
the  advantages  of  the  ball  bar  method  while  overcoming  the 
disadvantages  which  arise  from  constraining  the  manipulator 
end  effector. 
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2.  Physical  Description  of  the  Measurement  System 
a.  The  Wire  Potentiometer 

The  wire  potentiometer  used  in  this  experiment  is 
a  Celesco  model,  DV301-0050-111-III0 ,  and  is  illustrated  in 
Figure  28.  This  model  is  designed  to  produce  linear 
displacement  and  velocity  measurements  with  a  maximum  travel 
of  50  inches.  The  calibration  process  is  only  concerned  with 
displacement  measurements  which  are  transduced  by  means  of  a 
proportional  resistance  from  the  wiper  arm  of  0-500  n 
potentiometer . 


Figure  28.  Celesco  Wire  Potentiometer 

b.  Fixture  Design  for  Measurements  in  a  Volume 

As  noted  above,  the  wire  potentiometer  was 
designed  for  measurement  of  linear  motion.  To  effectively 
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utilize  this  device  in  a  calibration  application,  it  was 
necessary  to  design  a  fixture  capable  of  measuring  distances 
within  a  working  volume.  Several  factors  were  considered  when 
designing  the  fixtures: 

-  simplicity 

-  low  measurement  noise 

-  prevention  of  wear  and  deformation  of  the  wire 

-  mathematic  modelling 


The  fixture  designs  for  the  measurement  system  base  and  end 
effector  are  illustrated  in  Figures  29  and  30,  respectively. 


All  parts  were  constructed  from  aluminum.  The 
funnel  shaped  ports  were  cut  with  a  carbide  tipped  half-inch 
radius  beading  router  bit.  The  known  radius  of  curvature  of 
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Figure  30.  End  Effector  Fix±ure 


the  funnel  surface  can  easily  be  incorporated  into  a 
mathematical  model,  as  well  as  reduce  the  possibility  of  wire 
deformation.  The  funnel  surfaces  are  highly  polished,  which 
reduces  both  abrasive  wear  on  the  wire  and  measurement  noise 
by  allowing  the  wire  under  tension  to  align  itself  in  a 
minimum  length  configuration.  A  detailed  description  of  the 
geometry  and  wire  length  calculation  is  provided  in  the 
following  section.  A  tradeoff  between  smooth  travel  and 
minimal  play  at  the  throat  of  the  funnel  was  made  when 
determining  the  internal  passage  diameter. 
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3 .  Theory 

a.  Closed  Chain  Kinematic  Model 

As  in  the  two  preceding  experiments,  the 
manipulator  kinematic  parameters  are  the  same  as  those  listed 
in  Table  1.  The  measurement  system  kinematic  model  must  be 
developed  so  that  the  makeup  of  F"  and  are  known  which  then 
fixes  the  identifiable  parameters  in  T„°  and  T^^.  If  the  wire 
had  been  "perfectly  flexible  in  bending",  then  instead  of  the 
funnel  shaped  port,  the  wire  could  have  been  passed  through  a 
fixture  with  a  port  of  essentially  equal  diameter  as  the  wire, 
as  illustrated  in  the  planar  view  of  Figure  31. 


Figure  31.  Fixture  Design  for  a  "Perfectly  Flexible  in 
Bending"  Wire 
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In  this  case,  the  model  resembles  the  ball  joint 
model  described  previously.  Point  O  in  Figure  31  is  fixed  in 
translation  but  the  wire  is  "free"  to  assume  any  orientation 
above  the  fixture.  Point  O  could  then  be  defined  as  the 
measurement  system  reference  point.  With  a  similar  fixture 
mounted  on  the  end  effector  flange  of  the  PUMA,  a  second  point 
would  be  defined.  This  point  becomes  the  origin  of  . 
Summarizing,  f*  and  consist  of  well  defined  points  in  space 
but  no  axes  can  be  associated  with  the  measurement  system.  The 
same  concepts  hold  for  the  funnel  shaped  ports  with  the 
defined  point  now  located  at  the  throat  of  the  funnel. 
However,  due  to  the  axial  symmetry  of  the  funnel,  an  axis  can 
be  associated  with  the  F”  and  F®  as  illustrated  in  Figure  32. 
This  theoretically  identifiable  axis  was  not  included  in  the 
model  used  in  this  experiment  based  on  the  following 
assumptions : 

-  The  z  axis  illustrated  in  Figure  31  for  the  measurement 

system  base  and  end  effector  fixtures  would  be  nominally 
aligned  with  the  Zp  and  z^  axis  respectively. 

-  The  small  radius  of  curvature  would  make  identification  of 
any  nonalignment  unreliable. 

-  Any  errors  induced  would  be  sufficiently  small  that  they 
would  not  be  discernible  from  normal  meacurement  noise. 

Without  the  axes  included  in  the  model,  three  parameters  are 
identifiable  in  both  T„°  and  T5'.  As  stated  before,  any  three 
of  the  six  parameters  in  each  transformation  can  be  used  as 
long  as  one  parameter  is  a  translation.  The  nominal  values  of 
the  parameters  chosen  for  identification  are  typed  in  bold  in 
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Figure  32.  Axis  Defined  by  Funnel  Geometry 

the  kinematic  parameter  table.  Table  7.  Recall  that  although 
an  independent  set  of  axes  are  not  defined  at  either  F”  and  , 
by  virtue  of  the  transformations,  a  dependant  set  of  axes  is 
defined  and  can  be  referenced. 

As  developed,  the  total  model  consists  of  24 
identifiable  parameters.  For  a  given  configuration  of  the 
manipulator,  the  measured  quantity  is  the  length  of  wire 
between  the  origins  of  F"  and  .  Therefore,  in  the  absence  of 
noise,  a  minimum  of  24  measurements  are  required  to  adequately 
define  the  problem. 
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TABLE  7.  KINEMATIC  PARAMETER  TABLE  FOR  T„°  AND  T 


m  o 

M 

0 

0.0 

-135.0 

e 

0.0 

0.0 

0.0 

0 . 0 

X 

-75.0 

B 

-711.0 

0 . 0 

2 

552.0 

b.  Calculation  of  Wire  Length  from  Kinematic 

Parameters 

During  each  iteration  in  the  parameter 
identification  algorithm,  a  "predicted"  value  of  the  wire 
length  based  on  the  current  value  of  the  parameters  must  be 
computed  for  comparison.  The  position  and  orientation  of  the 
end  effector  fixture  reference  frame  with  respect  to  the 
measurement  frame  is  provided,  as  before,  by  the  T'^  matrix. 

As  noted  earlier,  the  wire  under  tension  will 
align  itself  in  a  minimum  length  configuration  between  the 
base  fixture  and  the  end  effector  fixture.  In  this 
configuration,  the  wire  departs  the  funnel  surfaces 
tangentially  and  in  a  direction  such  that  the  tangent  to 
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tangent  distance  is  minimum.  Figure  33  illustrates  the  wire 
configuration  for  an  arbitrary  manipulator  pose. 


Figxire  33.  Drawing  of  Base  and  End  Effector  Fixture 

Let  M  and  E  designate  the  origins  of  the 
measurement  and  end  effector  frames  respectively.  Define  TM 
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and  TE  as  the  points  of  departure  of  the  wire  from  the 
measurement  base  fixture  and  end  effector  fixture 

respectively.  Note  that  the  wire  departs  at  points  TM  and  TE 
tangentially  from  the  funnel  surface  and  will  be  referred  to 
as  the  tangent  points  for  the  remainder  of  this  discussion. 
Additionally,  define  the  z  axes  for  F”  and  to  lie  along  the 
axis  of  the  funnel  as  shown  previously  in  Figure  32.  The 
complexity  of  describing  the  wire  path  can  be  simplified 
somewhat  by  noting  the  following  feature.  If  the  wire  path  is 
projected  onto  the  x-y  plane  of  F"  and  then,  as  is  shown  in 
Figure  34,  the  projected  paths  M-TM-TE  and  E-TE-TM  in  each 
frame's  x-y  plane  is  a  straight  line.  Only  in  unigue 
configurations,  namely  each  frame's  z  axes  intersect,  will  the 
projected  path  M-TM-TE-E  form  a  straight  line. 

The  geometry  described  in  this  paragraph  is 
identical  for  either  frame  so  without  loss  of  generality  F”  is 
described.  Let  v  define  the  axis  in  the  xy  plane  of  F"  which 
describes  the  line  of  action  of  the  wire  path  projection  as 
shown  in  Figure  34.  Figure  35  illustrates  the  geometry  as 
viewed  in  the  vz  plane  of  the  frame.  If  the  x  and  y 
coordinates  of  the  tangent  point  TM  (tra^  and  tm^)  in  Figure  35 
are  known,  then  tm^  is  fixed  by  the  known  radius  of  curvature. 
Furthermore,  the  angle  0  can  be  determined  which  then  results 
in  a  solution  for  arc  length  O-TM.  Additionally,  the  angle  0 
can  be  calculated  which  then  fully  defines  the  direction  of 
line  segment  TM-TE.  From  this  analysis,  it  is  clear  that  the 
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Figure  34.  Projected  Paths  M-TM-TE  and  E-TE-TM 

wire  length  solution  is  a  function  of  the  x  and  y  components 
of  the  tangent  points  of  both  fixtures. 

L  =  L  itw^,  trrty,  te^,  tBy)  (43) 

Although  an  analytical  solution  to  this  problem 
most  likely  exists,  it  would  clearly  reduce  to  solving  4  non¬ 
linear  equations  with  the  4  unknowns  ultimately  requiring  some 
numerical  technique  to  obtain  a  solution.  Consequently,  an 
optimization  scheme  was  chosen  for  the  solution  method  at  the 


Figure  35.  V-Z  Planar  View  of  a  Fixture 

outset.  The  general  approach  was  to  develop  a  unit  vector  for 
the  tangential  direction  based  on  a  variable  x  and  y  position 
in  each  fixture's  reference  frame.  The  unit  vector  calculated 
in  the  end  effectors  reference  frame  is  then  transformed  into 
measurement  frame  coordinates.  These  vectors  are  designated  u„ 
and  Uj  and  their  component  by  component  sum  will  be  zero  when 
the  wire  is  in  a  minimum  length  configuration.  The  three  sums 
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form  three  functions  to  be  minimized.  However,  as  illustrated 
with  the  simple  example  in  Figure  36,  alignment  of  these  unit 
tangent  vectors  alone  does  not  sufficiently  constrain  the 
problem.  The  additional  constraint  required  is  that  the 
tangent  to  tangent  length  TM-TE  be  minimized  as  well.  A  fourth 
function  consisting  of  the  tangent  to  tangent  length  could 
then  be  included  for  minimization.  However,  as  stated  thus 
far,  the  problem  is  not  proportional  with  three  of  the 
functions  ideally  converging  to  zero,  and  the  fourth 
converging  to  a  comparatively  larger  number.  Scaling  the 
length  function  could  reduce  this  problem  to  an  acceptable 
level.  However,  the  following  method  eliminates  all 
disproportionate  aspects  between  the  functions  to  be 
minimized . 

Let  u^t  be  the  vector  describing  the  line  of  action 
between  TM  and  TE  as  illustrated  previously  in  Figure  36. 
Calculation  of  is  easily  accomplished  utilizing  the 

coordinates  of  TM„  and  TE„.  Now  the  component  by  component 
difference  between  u„  and  will  be  zero  when  the  wire  is  in 
a  minimum  length  configuration.  These  component  by  component 
differences  can  then  form  three  additional  functions  to  be 
minimized.  Note  that  the  difference  between  and  would 
have  worked  equally  well.  In  normal  problem  statement  form: 

-  Minimize:  S(fi(x))="  i=l,2,***,6 

-  Over:  x  =  ['XE,'y£,  "x„,  "y„  ] 

-  fi,  fj  and  fj  are  the  component  by  component  sum  of  u„  and 

Ur 
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Figure  36.  Possible  Solution  to  Alignment  of  Tangent  Vectors 


-  f«,  fs  and  f*  are  the  component  by  component  difference 
between  u„  and  u^t 

-  Recall  that  the  superscripts  on  the  elements  of  x  refer  to 
which  frame  the  variables  are  associated  with  and  the 
subscript  refers  to  which  frame  their  numerical  values  are 
referenced  to. 

With  a  numerical  solution  obtained,  the  wire  length  can  easily 
be  computed  from  the  components  of  x. 
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To  aid  convergence,  a  good  initial  estimate  of  the 
X  and  y  coordinates  for  each  frame  are  determined  in  the 
following  manner.  A  unit  vector  v  directed  from  the 
measurement  frame  origin  to  the  end  effector  frame  origin  is 
calculated  from  the  x,  y  and  z  position  elements  of  the 
matrix.  The  initial  x  and  y  values  in  the  measurement  frame 
are  chosen  as  the  funnel  radius  times  the  i  and  j  components 
of  this  unit  vector.  A  unit  vector  directed  from  the  end 
effector  reference  frame  origin  to  the  measurement  system 
reference  origin  is  simply  -v.  However,  this  vector  must  be 
described  in  the  end  effector's  reference  frame.  Calculation 
of  -Vj.  is  accomplished  by  Equation  44. 

(-v^)  (44) 

As  in  F”,  the  initial  x  and  y  value«5  in  are  calculated  by 
multiplying  the  i  and  j  components  of  by  the  funnel 

radius . 

Given  the  initial  or  updated  x  and  y  coordinates 
in  frame  coordinates,  calculation  of  the  tangent  vector  with 
respect  to  that  frame  proceeds  as  follows  and  is  identical  for 
both  frames.  As  before,  define  an  axis  v  that  lies  in  the 
frames  x-y  plane  and  intersects  the  point  defined  by  the 
current  values  of  x  and  y  and  the  frames  origin.  Figure  37  is 
a  view  of  the  plane  formed  by  the  v  and  z  axis.  Point  P  has 
coordinates  (x,y,0)  and  point  O  is  the  frame  origin.  The 
distance  from  O  to  P  is  found  from  Equation  45. 
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Figure  37 .  Second  View  of  v-z  Plane 


OP  =  \/x‘  +  y‘  (^5) 

The  length  of  line  segment  OQ  is  equal  to  the  radius,  r,  of 
the  funnel.  Calculation  of  the  side  PQ  of  the  right  triangle 
PQS  is  accomplished  with  Equation  46. 
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FU  =  r  -  OP 


(46) 


Since  the  length  of  SQ  is  the  radius,  then  length  SP  is 
calculated  by  Eguation  47  and  this  value  corresponds  to  the  z 
component  of  the  current  tangent  point. 

^  =  Vr"  -  PQ- 


The  angles  0  and  6  are  calculated  by 


4)  =  sin"’  (  ) 

(48) 

e  =  sin-M  — ) 
r 


The  angle  0  will  be  used  at  convergence  to  calculate  arclength 
OS.  The  unit  tangent  vector,  either  u„  or  u^  is  calculated  as 
shown  in  Equation  49. 


X  i  *  y  j  *  OPtan(t)  k 
v/x‘  +  y‘  +  (aPtan4))“ 


(49) 


4 .  Simulation 

a .  Introduction 

The  general  simulation  scheme  is  similar  ro  the 
preceding  calibration  methods.  A  flowchart  of  the  scheme  is 
illustrated  in  Figure  38.  The  programs  are  listed  in 
Appendices  D,  E  and  F.  Prior  to  a  description  of  the  main 
programs  In  the  simulation  process,  the  subroutines  LENGTH  and 
MINLENTH  which  calculate  the  wire  length  will  be  discussed. 
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Figure  38.  Wire  Potentiometer  Simulation  Flowchart 

b.  Subroutines  LENGTH  and  MINLENTH 

The  subroutine  LENGTH  and  its  subroutine  MINLENTH 
are  used  in  each  simulation  program  as  well  as  the 
experimental  version  of  WID6  and  the  program  COMP  which  is 
used  to  verify  the  accuracy  of  experimentally  calibrated 
parameters.  The  purpose  of  LENGTH  is  to  calculate  the  wire 
length  based  on  the  matrix  supplied  by  the  calling  program. 

LENGTH  initializes  the  ZXSSQ  option  parameters  and 
calculates  Xo,  the  ZXSSQ  variable  vector,  from  the  current 
frame  origins  as  described  earlier.  Upon  termination  of  ZXSSQ, 
the  overall  wire  length  is  calculated  and  passed  back  to  the 
calling  program.  In  program  WID6,  LENGTH  is  called  from  the 
subroutine  PUMA_ARM  which  is  similar  to  previously  described 
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versions.  Note  that  this  represents  an  inner  optimization 
loop.  Since  FORTRAN  is  not  a  recursive  language,  a  renamed 
version  of  the  ZXSSQ  source  code  was  required  so  that  the 
inner  loop  optimization  could  be  pei'formed. 

MINLENTH  is  the  "user  supplied  subroutine"  called 
by  ZXSSQ.  MINLENTH  is  passed  the  current  value  of  x.  The 
tangent  vectors  u„  and  are  calculated  as  well  as  u^t.  The 
coordinates  of  are  transformed  into  F"  coordinates  and  the 
six  error  functions  are  formed.  The  tangent  to  tangent  length 
and  the  elevation  angles  0  which  are  necessary  for  the  total 
wire  length  calculation  are  computed  and  passed  to  LENGTH  upon 
ZXSSQ  convergence. 

c.  The  Program  Wire 

The  function  of  the  program  WIRE  is  to  generate 
simulated  wire  length  and  joint  variable  data.  In  a  general 
sense,  it  performs  similarly  to  the  previously  described 
programs  JOINT  and  BLINSC.  The  nominal  kinematic  parameter 
table,  along  with  length  and  angular  offsets,  length  and 
angular  noise  levels,  and  che  total  number  of  simulated 
measurements  to  be  ge,ierated,  are  read  from  the  file 
INPUT.DAT.  The  length  and  angular  offsets  are  added  to  the 
corresponding  nominal  parameters.  These  values  will  be  used  in 
forward  kinematic  solution  calculations.  Comparable  to  BLINSC, 
data  simulation  will  require  an  inverse  kinematic  solution. 
Unlike  BLINSC,  the  end  effector  pose  must  be  constrained  to 
realistically  match  the  actual  experiment.  In  other  words,  as 
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viewed  froirs  the  end  effector  frame,  the  measurement  system 
base  frame  must  lie  above  the  x-y  plane  of  . 

The  first  step  in  the  process  is  to  establish  the 
origin  of  F^ .  This  random  point  is  developed  using  a  spherical 
coordinate  approach  where  random  values  of  0,  6  and  L, 
illustrated  in  Figure  39,  are  constructed  from  scaled  values 
output  from  a  Monte-Carlo  random  number  generator.  Note  that 
0  was  maintained  in  the  range  0°-80”  and  L  was  maintained  in 
the  range  of  100-1000  mm.  The  coordinates  0,  6  and  L  are  then 
converted  to  cartesian  coordinates  in  the  usual  way.  These 
three  values  become  part  of  the  ’’desired"  result  of  the 
forward  kinematic  solution. 


Figure  39.  Calculation  of  the  Origin  of  F' 


To  ensure  that  the  2  axis  of  was  in  a  feasible 
direction,  constrained  random  values  of  the  z  axis  direction 
cosines  were  incorporated  into  the  "desired"  solution.  This  is 
accomplished  by  first  calculating  a  unit  vector  v  directed 
from  the  origin  of  F^  to  the  origin  of  F”  in  F”  coordinates. 
Note  that  if  these  values  were  then  used  as  the  direction 
cosines  for  the  z  axis  in  T^,  then  the  2  axis  would  "point" 
directly  at  the  origin  of  F”.  These  direction  cosines  are 
randomly  adjusted  about  this  value  by  a  maximum  range  of  ±30°. 
Denoting  u  as  the  unit  vector  made  up  of  the  perturbed 

direction  cosines,  feasibility  is  checked  by  computing  the  dot 
product  of  u  and  v  which  must  be  less  than  90°.  These  six 
"desired"  values  correspond  to  the  last  two  columns  of  the 
matrix  and  are  passed  via  common  block  to  the  subroutine 
PUMA_ARM. 

The  ZXSSQ  vector  Xq  is  then  initialized  along  with 
the  ZXSSQ  option  parameters.  Subroutine  PUMA_ARM  is  called  by 
ZXSSQ  to  compute  the  error  functions.  As  before,  PUMA_ARM 
computes  the  forward  kinematic  solution,  T^,  based  on  the 

current  joint  variables  contained  in  the  vector  x.  The 
difference  between  the  upper  right  3x2  elements  of  the 

calculated  matrix  and  the  "desired"  values  form  six 

functions  to  be  minimized. 

After  ZXSSQ  termination,  the  residual  is  checked 
to  ensure  validity  of  the  calculated  pose.  If  the  solution  is 
valid,  the  wire  length  is  calculated  by  a  call  to  subroutine 
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LENGTH.  Random  noise  can  be  injected  into  the  wire  length  and 
joint  variables  at  this  point.  The  wire  length  and  joint 
variables  are  then  written  to  a  file  PUMA_SOLN.DAT.  The 
process  is  then  repeated  until  the  requisite  number  of 
simulated  measurements  have  been  generated, 
d.  The  Program  WID6 

WID6  performs  in  a  like  manner  to  preceding 
versions  of  ID6.  Minor  changes  have  been  made  to  reflect  the 
differences  in  the  identifiable  parameters.  The  only 
significant  difference  is  in  the  subroutine  PUMA_ARM. 

In  this  experiment,  the  measurement  consists  of 
the  length  of  wire  between  F”  and  which  is  a  function  of  the 
end  efrector  pose  and  the  fixture  geometry.  For  each  call  to 
PUMA_ARM,  a  forward  solution  is  calculated.  With 
calculated,  the  subroutine  LENGTH  is  called  and  a 
corresponding  wire  length  is  calculated  as  described 
previously.  The  difference  between  the  calculated  wire  length 
and  the  simulated  data  wire  length  form  a  single  error 
function.  This  process  is  repeated  until  N  error  functions 
have  been  calculated  where  N  is  the  number  of  measurements  in 
the  data  set. 

After  ZXSSQ  termination,  the  identified  parameters 
are  compared,  as  before,  with  the  known  parameters  and  these 
parameters  and  the  rms  position  error  are  written  to  a  file 
RESULT . DAT . 
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e.  The  Program  WVERIFY 

WVERIFY  performs  similarly  to  previous 
verification  programs.  The  kinematic  parameter  table  is  read 
from  the  file  RESULT.DAT.  A  second  set  of  simulated  joint 
variable  and  wire  length  data,  generated  by  WIRE,  is  read  from 
the  file  POSVER.DAT.  A  forward  solution  and  then  the 
corresponding  wire  length  is  calculated  for  each  set  of  joint 
variables.  The  difference  between  the  calculated  lengths  and 
the  corresponding  simulated  length  measurements  is  computed. 
An  rms  value  for  all  errors  is  calculated  and  written  to  the 
terminal  screen. 

5 .  Experiment 

a.  Calibration  of  the  Potentiometer 

A  model  SE-2000  signal  conditioner  and  digital 
display  unit  was  provided  with  the  wire  potentiometer. 
However,  the  digital  display  was  only  capable  of  0.01  inch 
accuracy  which  was  inferior  to  the  desired  accuracy.  The  unit 
does  provide  a  conditioned  0-10  volt  analog  output  for 
displacement  measurement.  It  was  hoped  that  the  potentiometer 
was  capable  of  greater  precision  so  it  was  calibrated  in  the 
following  manner. 

The  end  effector  fixture  was  mounted  in  the  chuck 
of  a  lathe  and  the  base  fixture  mounted  to  the  lathe  carriage. 
The  fixtures  were  aligned  so  that  the  wire  was  normal  to  both 
fixture  upper  surfaces  as  illustrated  in  Figure  40.  The 
carriage  was  then  positioned  so  that  the  upper  fixture 
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surfaces  were  in  light  contact.  The  potentiometer  was 
connected  to  the  SE-2000  and  the  SE-2000  analog  output  was 
connected  to  a  5  1/2  digit  DVM.  The  "zero"  was  adjusted  on  the 
SE-2000  for  a  zero  volt  reading  on  the  DVM.  Digital  display, 
accurate  tc  0.001  mm,  of  lathe  carriage  travel  is  provided  by 
an  Acurite  III  display  unit.  Voltage  and  length  readings  were 
recorded  at  50  mm  intervals.  Figure  41  is  a  Displacemnt  vs 
Voltage  plot  of  the  data.  As  shown,  a  linear  relationship 
results  and  Equation  50  is  the  linear  best  fit  of  the  data. 
Figure  4  2  is  a  plot  of  the  deviation  of  the  data  from  the 
linear  best  fit  over  the  length  of  the  wire.  The  rms  error  in 
the  deviation  from  Equation  50  is  0.22  mm.  Although  the 
potentiometer  was  not  nearly  as  precise  as  desired,  simulation 
with  this  level  of  noise  still  demonstrated  robust  convergence 
with  the  overall  error  approaching  the  accuracy  of  the 
measurement  device  which  is  less  than  the  repeatability  of  the 
PUMA. 

L  =  0.12659V  -  0.490203  (50) 

b.  Data  Acquisition 

A  short  data  acquisition  program  was  written  to 
convert  voltage  measurements  to  millimeters  using  the  linear 
relationship  derived  from  the  potentiometer  calibration.  This 
file  then  stored  each  measurement  along  with  the  corresponding 
joint  variable  data  in  a  file  titled  PUMA_POS.DAT. 
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Figure  40.  Calibration  of  the  Potentiometer 

The  base  fixture  was  secured  to  the  work  table  as 
illustrated  in  Figure  43.  As  in  the  calibration  setup,  the 
potentiometer  was  connected  to  the  SE-2000,  and  the  analog 
output  of  that  unit  was  connected  to  the  5  1/2  digit  DVM. 
After  sufficient  equipment  warm-up  and  with  the  end  effector 
fixture  resting  on  the  base  fixture,  the  voltage  output  was 
adjusted  for  a  zero  reading.  The  end  effector  fixture  was  then 
bolted  to  the  PUMA.  The  PUMA  was  placed  in  a  wide  variety  of 
joint  variable  configurations  and  data  recorded.  The  length  of 
the  wire  was  sufficient  to  enable  the  PUMA  to  be  placed  in  all 
four  "arm  configurations",  lio  measurements  were  taken  in 
approximately  four  hours  by  one  operator  entering  data  at  a 
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Figure  41.  Plot  of  Wire  Potentiometer  Calibration  Data 

computer  terminal  and  operating  the  PUMA  with  the  teach 
pendant . 

c.  Parameter  Identification  and  Verification 

The  entire  data  set  was  used  for  parameter 
identification  in  a  version  of  WID6  modified  for  experimental 
data.  The  nominal  and  identified  kinematic  parameters  are 
listed  in  Table  8  where  the  length  and  angular  values  are  in 
units  of  millimeters  and  degrees,  respectively. 
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Figure  43.  Wire  Potentiometer  Calibration 
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Table  8.  NOMINAL  AND  CALIBRATED  KINEMATIC  PARAMETERS 


Nominal 

Calibrated 

Xm 

-75.0 

-75.118 

Yh 

-711.0 

-724.381 

Zh 

552.0 

544.183 

ai 

0.0 

-0.489 

-90.0 

-89.944 

66^ 

0.0 

-0.523 

a. 

431.85 

431.958 

0.0 

-0.023 

B, 

0.0 

-0.037 

603 

0.0 

-1.272 

d. 

149.09 

149.340 

a3 

-20.33 

-18.735 

a. 

90.0 

90.125 

60. 

0.0 

-0.906 

d. 

433.0 

432.726 

a. 

0.0 

0.229 

-90.0 

-90.263 

60^ 

0.0 

1.248 

ds 

0.0 

-0.532 

a^ 

0.0 

0.136 

Cls 

90.0 

90.254 

<fr 

-135.0 

-131.712 

Xr 

-76.2 

-75.914 

z. 

76.2 

76.392 

The  data  was  then  divided  into  two  set's  of  equal 
size.  The  first  set  of  data  was  used  in  WID6  for  parameter 
identification.  The  second  set  of  data  was  used  for 
verification  by  computing  a  forward  solution  based  on  the 
identified  parameters  and  the  second  sets  joint  variables.  A 
set  of  wire  lengths  was  calculated  based  on  the  forward 
kinematic  solution  results.  These  wire  lengths  were  then 


compared  with  their  corresponding  measured  values  and  an  rras 
error  of  0.490  mm  was  calculated. 
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IV.  DISCUSSION  OF  RESULTS 


A.  COMPARATIVE  ANALYSIS  OF  EXPERIMENTAL  RESULTS 

All  three  calibration  techniques  resulted  in  an 
improvement  in  the  accuracy  of  the  PUMA  560.  The  resulting 
accuracy  for  each  method  is  listed  in  Table  9. 

Table  9.  CALIBRATION  ACCURACY 


COORDINATE 

MODIFIED  LINEAR 

WIRE 

MEASURING  MACHINE 

SLIDE 

POTENTIOMETER 

0.3  mm 

0.744  mm 

0.490  mm 

Only  the  18  parameters  of  the  PUMA  are  common  to  each 
method's  closed  chain  kinematic  model.  The  calibrated  values 
for  each  method  along  with  the  nominal  parameters  are  listed 
in  Table  10  where  the  length  and  angular  parameters  have  units 
of  millimeters  anv'  degrees  respectively.  It  should  be  noted 
that  since  the  actual  values  are  unknown,  little  can  be  gained 
by  direct  comparison  of  parameter  values.  Additionally, 
repairs  were  affected  to  the  manipulator  between  the  CMM 
experiment  and  the  other  two  methods.  The  abbreviations  CMM, 
MLS  and  WP  are  used  in  the  column  headings  for  the  Coordinate 
Measuring  Machine,  Modified  Linear  Slide  and  Wire 
Potentiometer  methods  respectively. 
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TABLE  10.  CALIBRATED  PARAMETERS 


NOMINAL 

CMM 

MLS 

WP 

0.00 

-0.049 

-0.096 

-0.489 

-90.00 

-89.998 

-89.823 

-89.944 

66, 

0.00 

-0.489 

-0.340 

-0.523 

431 . 85 

432.122 

431.123 

431.958 

a. 

0 . 00 

-0.030 

0.580 

-0.023 

B, 

0.00 

-0.015 

0.485 

-0.037 

66, 

0.00 

-1.207 

-0 . 993 

-1.272 

d. 

149.09 

149.146 

146.028 

149.340 

a3 

-20.33 

-19.227 

-20.255 

-18.735 

0:3 

90 . 00 

90.051 

90.415 

90.125 

66. 

0 . 00 

-0.914 

-1.089 

-0.906 

d. 

433.00 

432.889 

434.095 

432.726 

a. 

0.00 

0.004 

0.074 

0.229 

Q4 

-90.00 

-89.991 

-90.244 

-90.263 

66„ 

0.00 

2.236 

1.293 

1.248 

dj 

0 . 00 

-0.663 

-0.863 

-0.532 

as 

0 .00 

-0.026 

-0.175 

0.136 

a,. 

90.00 

89.934 

89.905 

90.254 

Since  all  three  methods  resulted  in  an  accuracy 
approaching  or  equalling  the  repeatability  of  the  manipulator, 
other  factors  take  on  added  significance  when  making 
comparisons.  Some  of  these  factors  are  somewhat  qualitative 
and  they  are  discussed  in  the  following  paragraphs. 

Although  the  CMM  base  was  used  for  the  Modified  Linear 
Slide  method,  it  was  used  to  simulate  a  device  which  is 
assumed  could  be  manufactured  with  similar  measurement 
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accuracy  and  characteristics  but  at  a  lower  cost.  With  this  in 
mind,  the  CMM  is  clearly  the  most  expensive  method  of  the 
three.  The  Wire  Potentiometer  Method  is  certainly  the  least 
expensive . 

The  compact  size  of  the  Wire  Potentiometer  fixtures  make 
them  very  portable  and  durable  and  hence,  well  suited  to  an 
industrial  enviroment.  The  CMM  on  the  other  hand,  is  clearly 
better  suited  for  laboratory  applications.  The  Linear  Slide, 
as  it  did  in  terms  of  cost,  would  seem  to  fall  somewhere 
between  the  other  two  methods.  Although  a  rugged  device  could 
be  designed,  the  need  for  a  stiff  slide  would  result  in  a  loss 
of  portability.  A  stiff  slide  is  necessary  to  reduce  noise 
induced  by  flexure  under  the  weight  of  the  manipulator. 

Both  the  CMM  and  Wire  Potentiometer  methods  are  capable  of 
measurements  of  the  PUMA  in  all  manipulator  "arm 
configurations"  without  any  additional  considerations. 
Conversely,  great  care  must  be  exercised  when  switching 
between  arm  configurations  when  calibrating  with  the  Modified 
Linear  Slide  Method  since  the  end  effector  must  be  detached. 

Even  with  automated  data  acquisition  of  PUMA  joint 
variable  data  and  CMM  position  data,  it  would  still  require 
one  operator  nearly  10  minutes  to  measure  one  pose  utilizing 
the  CMM.  Two  operators  are  required  for  the  Modified  Linear 
Slide  Method  due  to  the  need  to  support  the  PUMA  while  in  Free 
Mode.  Automated  data  acquisition  would  greatly  reduce  the 
overall  time  lequired  for  this  method.  The  wire  potentiometer 
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method  offers  significant  advantages  in  terms  of  data 
acquisition  as  well.  Automated  data  acquisition  would 
significantly  reduce  the  amount  of  time  required  to  gather 
data.  Additionally,  the  manipulator  could  be  programmed  to 
move  through  a  series  of  predetermined  poses  thus  enabling  the 
entire  process  to  be  automated.  The  resulting  calibration 
would  require  only  one  operator  about  ten  minutes  to  both  set 
up  and  dismantle  the  system.  Furthermore,  an  extensive  data 
base  could  be  collected  in  a  manner  of  minutes  without 
operator  intervention. 

B.  GENERAL  OBSERVATIONS  FROM  EXPERIMENTS 

The  original  Linear  Slide  method  seemed  to  offer  a  number 
of  advantages  over  the  CKM  method.  Since  the  end  effector  is 
fixed  in  orientation,  six  "knowns"  are  acquired  for  each 
displacement  measurement.  In  contrast,  the  CMM  method  requires 
12  position  measurements  to  "measure”  full  pose  or  acquire  6 
knowns.  Since  each  position  measurement  is  made  with  the  same 
accuracy  in  both  methods,  it  would  seem  that  the  Linear  Slide 
method  offers  a  significant  reduction  in  measurement  noise. 
However,  the  resulting  calibration  accuracy  using  the  Linear 
Slide  was  three  times  less  than  achieved  with  the  CMM  method 
[Ref.  13].  This  "loss"  of  accuracy  was  attributed  to 
additional  noise  induced  by  internal  loading  effects,  slide 
flexure  under  the  manipulator's  weight  and  insufficient  joint 
variable  excitation. 


109 


Two  methods  were  considered  for  improving  the  joint 
excitation.  Appendix  G  describes  a  method  for  determining  an 
optimum  position  and  orientation  of  the  slide.  Optimum  in  this 
case  is  defined  as  a  slide  position  and  orientation  which 
results  in  maximum  excursion  of  all  six  joint  variables  as  the 
end  effector  travels  along  the  slide  length.  The  Modified 
Linear  Slide  method  offered  a  significant  improvement  in  joint 
variable  excitation  although  this  method  results  in  a 
reduction  by  one  half  in  the  number  of  knowns  for  each 
displacement  measurement.  Although  this  modification  resulted 
in  improvement  in  the  calibration  accuracy,  the  accuracy  was 
still  less  than  that  achieved  with  the  CMM  method  for 
comparable  si2ed  data  sets.  This  reduction  in  accuracy  may  be 
a  consequence  of  mechanical  noise  as  suggested  previously. 
However,  simulation  studies  by  Pathre  and  Oriels  [Ref.  14] 
suggest  that  trajectory  or  pathlike  motion  of  the  end  effector 
may  result  in  a  less  accurate  calibration  than  if  random 
motion  of  the  end  effector  is  utilized.  This  is  an  issue  for 
further  research. 

The  Wire  Potentiometer  method  would  seem  to  support  the 
benefits  of  both  large  joint  excitation  and  "randoiti”  pose 
measurements.  Despite  its  noisy  characteristics  and 
comparatively  lower  accuracy,  a  more  accurate  calibration  was 
achieved.  This  suggests  that  a  tradeoff  may  exist  when 
considering  mechanical  constraining  type  measurement  systems. 
In  general,  additional  end  effector  constraints  increase  the 
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number  of  knowns  for  each  manipulator  pose  as  well  as  a 
reduction  in  noise.  However,  the  additional  constraints 
frequently  result  in  a  loss  of  some  "dimension"  of  the  problem 
and  a  subsequent  loss  in  calibration  accuracy. 

C.  OBSERVATIONS  REGARDING  MEASUREMENT  SYSTEMS  WITHIN  CLOSED 
CHAIN  KINEMATIC  MODELS 
1 .  INTRODUCTION 

As  stated  earlier,  the  number  of  identifiable 
parameters,  N,  in  a  "complete"  manipulator  kinematic  model  is 
given  by  Equation  31  which  is  repeated  here 

N  =  2P+4i?+6  (51) 

where  P  is  the  number  of  prismatic  joints  and  R  is  the  number 
of  rotary  joints.  For  the  PUMA  560,  N  is  equal  to  30.  This 
model  assumes  a  reference  frame  external  to  the  manipulator. 
The  advantage  of  this  model  is  that  it  offers  a  convenient 
method  of  referencing  other  objects  and  tools  within  the 
working  volume  of  the  manipulator.  However,  a  closed  chain 
kinematic  model  incorporating  a  measurement  system  or  device 
may  not  provide  sufficient  information  to  fully  define  a 
"complete"  model.  Define  is  used  in  the  sense  that  all  the 
parameters  of  the  model  are  identifiable.  In  general,  a 
manipulator  kinematic  model  incorporating  a  measurement  system 
will  have  no  more  than  N  identifiable  parameters  and  clearly 
a  model  with  greater  than  N  parameters  contains  parameters 
unnecessary  for  "completeness". 
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The  process  of  determining  the  number  of  identifiable 
parameters  in  models  incorporating  a  variety  of  measurement 
systems  all  to  frequently  results  in  an  iterative  process.  For 
the  PUMA  and  a  particular  measuring  system,  the  process 
started  with  a  30  parameter  model  followed  by  simulation 
studies  which  resulted  in  non-convergence  if  in  fact 
dependencies  existed  between  parameters.  The  model  was  then 
redefined  by  eliminating  parameters  based  on  the  numerical 
results,  "first  principles"  analysis  or  intuition.  Further 
simulation  studies  were  then  conducted  until  the  correct  model 
was  developed.  Clearly,  a  systematic  approach,  such  as 
Denavit-Hartenburg  is  to  manipulator  kinematics,  would  be 
advantageous  for  modelling  closed  loop  kinematic  chains 
incorporating  different  measurement  systems. 

The  ambiguities  which  can  exist  are  strictly 
attributable  to  the  measurement  system.  If  the  remainder  of 
the  model  is  properly  defined,  then  much  of  the  difficulty  is 
eliminated.  The  general  process  is  illustrated  in  Figure  44. 
The  process  consists  of  properly  defining  a  kinematic  model  of 
the  manipulator.  Proper  is  used  in  the  sense  that  the  model 
eliminates  possible  parameter  dependencies  in  transformations 
to  frames  external  to  the  manipulator.  From  this  point 
forward,  the  term  "manipulator  kinematic  model"  or  simply 
"manipulator  kinematics"  will  refer  to  such  a  model.  The 
proper  kinematic  model,  in  the  same  sense  as  before,  of  the 
measurement  system  must  then  be  defined.  The  transformations 
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T„°  and  which  connect  the  two  models  are  then  easily 

defined. 


MANIPULATOR 

KINEMATICS 


MEASUREMENT 

SYSTEM 

KINEMATICS 


Figure  44.  General  Model  Development 

2.  Manipulator  Kinematic  Modelling 

For  general  link  to  link  transformations  within  a 
manipulator,  the  Denavit-Hartenburg  or  Modified  Denavit- 
Hartenburg  method  provides  a  model  in  which  all  parameters  are 
identifiable.  Care  must  be  exercised  though  when  defining  both 
the  base  frame  of  the  manipulator  and  what  is  defined  as  the 
"end"  of  the  manipulator  kinematics.  For  an  n  link 
manipulator,  the  "end"  of  the  manipulator  kinematics  should  be 
defined  by  frame  n-1.  This  frame  is  chosen  due  to  the  fact 
that  it  is  the  last  frame  uniquely  defined  by  the  manipulator 
geometry.  Recall  though  that  prismatic  joints  may  lead  to 
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ambiguity  in  this  frame's  location  as  well.  A  unique 
manipulator  base  frame  defined  by  the  robots  geometry  is 
required  as  well.  For  a  rotary  joint  one,  in  which  the  joint 
one  axis  is  not  parallel  with  joint  axis  two,  this  is 
accomplished  as  described  earlier  for  the  PUMA.  The  resulting 
transformation  to  frame  one  contains  only  two  identifiable 
parameters.  It  can  be  shown  that  a  unique  base  frame  can  be 
defined  which  results  in  a  similar  reduction  in  the  number  of 
identifiable  parameters  by  two  in  models  with  parallel  or 
nearly  parallel  joint  axes  one  and  two  or  a  prismatic  joint 
one.  This  can  also  be  seen  by  studying  the  development  of 
Equation  51  [Ref.  15].  The  result  is  that  the  number  of 
identifiable  parameters,  K,  in  a  manipulator  kinematic  model 
as  defined  here  is  given  by  Equation  52  where  P  and  R  are 
defined  as  before. 

K  =  2  P  +  4  R  -  6  (52) 

For  the  six  rotary  joint  PUMA  560,  the  number  of  identifiable 
parameters  is  18  as  developed  earlier  and  listed  in  Table  1. 

3.  Measurement  System  Modelling 

The  preceding  model  development  and  analysis  leads  to 
a  lower  bound  on  the  number  of  identifiable  parameters  in  a 
closed  chain  kinematics  model  incorporating  a  measurement 
device  or  system.  The  last  or  end  frame  in  a  manipulator 
kinematic  model,  F'”^  is  a  unique  fixed  frame  for  a  given  set 
of  joint  variables.  The  base  frame  is  also  unique.  Therefore, 
six  unique  parameters,  three  rotations  and  three  translations. 


are  required  to  transform  between  the  two  frames.  Any  number 
of  parameters  less  than  six  would  imply  dependencies  within 
the  manipulator  kinematics  which  are  known  to  be  unique. 
Therefore,  the  minimum  number  of  parameters,  M,  in  the  closed 
chain  model  is 


M  =  2  P  +  4  i? 


(53) 


Summarizinq,  the  number  of  identifiable  parameters,  n,  in  the 
closed  chain  kinematic  model  is  bounded  by  M  and  N. 

(54) 

M  ^  n  ^  N 


Applying  Equation  54  to  the  PUMA,  the  number  of  identifiable 
parameters  is  between  24  and  30  inclusive.  In  terms  of  the 
measurement  system  kinematic,  the  foregoing  states  that  the 
model  will  consist  of  at  least  six  and  no  more  than  12 
constraints . 

4.  Linking  the  Measurement  and  Manipulator  Models 

If  the  number  of  constraints  in  the  measurement  system 
model  is  less  than  12,  then  the  model  will  contain  geometric 
quantities  such  as  points  or  axes  which  can  be  thought  of  as 
"reduced  order  frames".  As  described  in  the  section  Other 
Special  Cases,  transformations  between  frames  and  "reduced 
order  frames"  are  easily  developed  and  clearly  indicate 
dependant  parameters . 
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5.  Case  Studies 


A  number  of  calibration  methods,  both  performed  and 
proposed,  were  studied  in  detail  in  order  to  redefine  the 
overall  model  in  the  form  of  Figure  44.  Specific  emphasis  was 
placed  on  the  measurement  system  model  development  with  a  goal 
of  "standardizing"  this  process.  Each  method  assumes 
calibration  of  a  PUMA  560  and  thus  the  manipulator  kinematic 
model  consists  of  the  previously  defined  18  parameters, 
a.  The  Coordinate  Measuring  Machine 

The  Coordinate  Measuring  Machine  method  described 
earlier  was  studied  and  the  model  was  then  developed  in  the 
form  of  Figure  44.  The  measurement  system,  which  consist  of 
the  CMM  and  the  precalibrated  tooling  ball  end  effector,  are 
illustrated  in  Figure  45  and  its  kinematic  model  is  briefly 
restated  here.  Since  the  orthogonal  axes  of  the  CMM 
independently  define  a  reference  orientation  and  the  position 
measurements  can  be  made  with  respect  to  a  zero  reference,  an 
independent  reference  frame  is  defined.  Through  a  series  of 
position  measurements  of  the  calibrated  tooling  ball  end 
effector,  full  pose  of  the  end  effector  is  defined  as  well. 
Therefore,  the  measurement  system  kinematic  model  consists  of 
two  fully  defined  and  independent  frames  for  a  total  of  12 
constraints . 

All  12  parameters  are  identifiable  in  the 
transformations  linking  the  manipulator  and  the  measurement 
system  and  hence  n=30  for  the  overall  model.  Table  11  lists 
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the  parameters  of  T„°  and  which  are  all  typed  in  bold  since 
all  12  are  identifiable.  Figure  46  illustrates  the  CMM  closed 
chain  kinematic  model. 

b.  The  Ball  Bar 

The  ball  bar  described  earlier  consist  of  a  rigid 
bar  of  fixed  length  with  a  ball  joint  mounted  at  each  end  and 
is  illustrated  in  Figure  47.  One  ball  joint  is  fixed  in  the 
manipulator's  workspace  and  the  other  is  attached  to  the 
PUMA'S  end  effector  flange.  Each  ball  joint  has  three 
constraints,  fixed  translations,  and  consequently  is  capable 
of  defining  a  point.  The  three  degrees  of  freedom,  three 
rotations,  prevent  associating  an  orientation  at  either  joint. 
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Consequently,  the  measurement  system  model  simply  consist  of 
two  points. 


Figure  47.  Ball  Bar 

As  noted  earlier,  three  parameters  are  required  to 
transform  from  a  frame  to  a  point  and  one  of  these  parameters 
must  be  a  translation.  The  overall  model  must  then  consist  of 
24  identifiable  parameters.  A  logical  choice  of  parameters  for 
identification  are  indicated  in  bold  in  Table  12.  The 
unidentifiable  parameters  of  the  RPYT  transformation  are 
defined  to  be  zero  as  indicated.  The  closed  chain  kinematic 
model  is  illustrated  in  Figure  48. 
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c.  The  Linear  Slide 


The  linear  slide  measurement  system  consists  of 
the  lower  base  assembly  of  the  CMM.  The  y  axis  post  is  removed 
and  the  PUMA  end  effector  is  bolted  in  this  location.  The  PUMA 
and  slide  are  illustrated  in  Figure  49.  The  end  effector 
orientation,  since  it  is  bolted  to  the  slide,  is  fixed.  Motion 
along  the  slide  defines  a  direction.  Relative  displacement 
measurements  are  available  from  the  CMM  display  unit.  However, 
much  like  the  prismatic  joints  in  manipulator  kinematics,  no 
specific  point  is  defined  by  the  geometry.  In  an  analogous 
fashion,  an  axis  can  be  specified  at  the  "next"  or  "last" 
defined  coordinate  frame.  Two  choices  are  available,  either 
the  PUMA  base  frame  or  frame  5.  Defining  the  axis  at  frame  5 
more  closely  resembles  the  physical  system  and  so  this  frame 
is  chosen.  This  fixes  coincident  with  F®.  With  an  axis  now 
defined,  a  unique  point  on  this  axis  can  be  defined  by  the 
common  normal  between  joint  axis  one  and  this  axis.  Another 
fixed  point  on  the  axis  is  defined  as  well  by  the  relative 
displacement  measurements.  The  origin  of  F"  can  be  placed  at 
this  point  noting  that  only  one  direction  or  orientation 
constraint  is  specified.  Summarizing,  the  orientation  of  is 
specified  but  its  origin  is  not  unique.  The  origin  of  F”  is 
specified  as  well  as  one  orientation. 

To  transform  from  a  frame  to  a  point  on  axis 
requires  three  translations  (point  to  point)  and  two  rotations 
for  an  axis  alignment.  Since  F^  is  coincident  with  F^,  only 
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Figure  49.  The  PUMA  and  Linear  Slide 


three  rotations  are  necessary  to  transforin  between  these  two 
frames.  Therefore,  eight  parameters  are  necessary  for  the 
transformations  between  the  manipulator  kinematics  and  the 
measurement  kinematics  which  results  in  a  26  parameter  model. 
Table  13  lists  the  parameters  in  these  two  transformations 
with  the  identifiable  parameters  in  bold  and  the 
unidentifiable  parameters  defined  to  be  zero  as  before.  The 
model  is  illustrated  in  Figure  50. 
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Table  13. 


T„°  and  Tj,'  Kinematic  Parameter  Table 
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d.  The  Wire  Potentiometer 

The  Wire  Potentiometer  method  was  described  in 
detail  earlier  and  is  briefly  described  here.  The  wire 
potentiometer  provides  a  resistance  which  is  proportional  to 
the  amount  of  wire  extracted  from  the  device.  These  devices 
are  designed  for  linear  displacement  measurements.  In  order  to 
provide  displacement  measurements  in  a  volume,  the  two 
fixtures  illustrated  in  Figure  51  were  designed.  The  funnel 
shaped  ports  prevent  wire  deformation  and  each  defines  an 
axis.  The  throat  of  the  funnel  defines  a  fixed  point  in  a 
similar  fashion  to  a  ball  joint.  Therefore,  the  measurement 
system  consists  of  two  points  and  an  axis  through  each. 

As  noted  earlier,  five  parameters  are  required  to 
transform  between  a  frame  and  a  point  on  an  axis.  Therefore, 
a  model  with  28  identifiable  parameter  results.  Note  that  due 
to  the  small  size  of  the  funnel  ports  used  in  the  previously 
described  experiment,  both  axes  were  assumed  not  to  be 
identifiable.  In  this  case,  the  model  reduces  to  a  24 
parameter  model.  Table  14  lists  both  the  identifiable 
parameters  and  unidentifiable  as  before  for  the  28  parameter 
model.  Note  that  other  combinations  are  possible.  Figure  52 
illustrates  the  kinematic  model. 
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Figure  52.  Wire  Potentiometer  Kinematic  Model 


126 


e.  Single  Theodolite 

Calibration  of  a  PUMA  560  was  performed  by 
Whitney,  Lozinski  and  Rourke  using  a  single  theodolite  [Ref. 
16].  The  single  theodolite  measurement  system  is  composed  of 
a  theodolite  and  a  target  fixed  on  the  manipulators  endpoint. 
The  theodolite  is  capable  of  accurately  measuring  an  azimuth 
and  elevation  angle  of  an  object  along  its  line  of  sight.  The 
target  defines  a  point  in  space.  The  intersection  of  the 
azimuth  and  elevation  angles  defines  a  point  and  since  these 
are  measured  with  respect  to  a  zero  reference,  a  specific 
orientation  is  defined  at  this  point.  This  relationship  is 
illustrated  in  Figure  53.  Summarizing,  the  measurement  system 
kinematic  model  consists  of  a  point  at  and  a  fully  defined 
frame  at  F”. 

Six  parameters  are  obviously  necessary  to 
transform  from  F"  to  F°.  The  transformation  requires  three 
of  parameters  and  as  usual,  one  must  be  a  translation.  With 
these  9  identifiable  parameters,  the  resulting  closed  chain 
kinematic  model  consists  of  27  parameters.  Table  15  lists  one 
possible  combination  of  identifiable  and  unidentifiable 
parameters.  Figure  54  illustrates  the  kinematic  model  for  the 
single  theodolite  closed  chain. 
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Figure  53.  Theodolite  Measurement  System 
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f.  Three  Wire  Potentiometer 

The  three  wire  potentiometer  method  utilizes  three 
wire  potentiometers  placed  in  a  known  or  calibrated  triangular 
arrangement.  The  end  effector  fixture  consists  of  a  triangular 
shaped  plate  with  three  funnel  shaped  ports  again  placed  in  a 
calibrated  triangular  arrangement.  A  sketch  of  the  system  is 
shown  in  Figure  55.  The  known  triangular  arrangement  defines 
a  frame  at  both  the  measurement  system  base  and  end  effector. 


Figure  55.  Three  Wire  Potentiometer  Measurement  System 

With  frames  defined  at  both  F"  and  ,  the 
resulting  closed  chain  kinematic  model  contains  all  30 
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parameters  of  the  "complete"  model.  The  kinematic  parameter 
table  for  T„''  and  is  the  same  as  Table  11  and  the  closed 
chain  kinematic  model  is  the  same  as  Figure  46. 
g.  Planar  Motion 

This  measurement  system  consist  of  a  smooth  flat 
plate  placed  in  the  manipulators  workspace  and  an  end  effector 
with  a  single  precision  tooling  ball.  The  tooling  ball  is 
placed  in  contact  with  the  plate  in  a  variety  of  joint 
variable  configurations  and  at  arbitrary  locations  on  the 
plate.  In  this  measurement  system,  the  tooling  ball  defines  a 
point  and  this  is  the  makeup  of  F^.  The  "planar"  motion  of 
this  point  defines  a  plane.  A  unique  point  can  be  defined  at 
the  intersection  of  the  plate  "plane"  and  joint  axis  one.  A 
unique  direction  perpendicular  to  the  plate  is  specified  at 
this  point  but  no  additional  orientation  is  specified. 
Consequently,  F"  is  composed  of  a  point  on  an  axis.  Note  that 
F"  lies  on  joint  axis  one. 

Two  rotations  and  one  translation  are  required  in 
T„°.  The  rotations  are  necessary  for  axis  alignment  and  then 
the  translation  along  joint  axis  one  places  F"  at  F°.  Three 
parameters  can  be  identified  in  Ts',  a  frame  to  point 
transformation,  and  one  of  these  must  be  a  translation  as 
usual.  The  resulting  closed  chain  kinematic  model  then 
consists  of  24  identifiable  parameters.  Table  16  indicates  one 
possible  combination  of  identifiable  parameters.  Figure  56 
depicts  the  closed  chain  kinematic  model.  If  the  tooling  ball 
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Figure  66.  Planar  Motion  Closed  Chain  Kinematic  Model 


6.  Discussion  of  Case  Studies 

There  are,  in  general,  two  broad  categories  of 
measurement  systems  used  in  manipulator  calibration: 

-  Those  which  mechanically  constrain  the  manipulator 

-  Those  which  do  not  mechanically  constrain  the  manipulator 
The  non-constraining  type  systems  are  typically  easier  to 
model  and  these  will  be  discussed  first. 

One  reason  that  non-constraining  type  measurement 
systems  are  usually  easier  to  model  is  that  a  measurement 
system  reference  frame  is  typically  better  defined.  The  second 
reason  is  that  the  measured  quantities  are  often  the  more 
familiar  cartesian  or  spherical  coordinates  of  a  point  or 
points  on  the  end  effector. 
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Conversely,  the  location  and  any  associated 
orientation  of  a  reference  frame  for  constraining  type 
measurement  systems  is  frequently  not  obvious.  In  fact,  it  is 
often  not  defined  until  the  position  of  the  end  effector  frame 
is  defined.  In  this  case,  this  frame  is  not  really  a  reference 
at  all,  but  it  is  convenient  for  symmetry  to  refer  to  it  as 
such.  Additionally,  the  measured  quantities  may  consist  of 
some  known  spatial  relationship  such  as  a  path  or  surface  as 
opposed  to  more  familiar  lengths  and  angular  displacement. 

Although  no  foolproof  cookbook  approach  to  model 
development  seems  to  exist,  a  set  of  guidelines  can  be 
established.  A  generalized  process  for  model  development  is 
illustrated  in  the  flowchart  in  Figure  57.  The  first  step  is 
to  determine  which  of  the  two  broad  categories  the  measurement 
system  belongs.  Even  this  step  is  not  always  trivial  since  a 
measurement  system  such  as  the  wire  potentiometer  could  be 
arguably  placed  in  either  category. 

As  stated  earlier,  most  non-constraining  measurement 
systems  have  relatively  well  defined  reference  frames. 
However,  it  is  noted  that  it  is  not  a  trivial  task  to  define 
why,  for  example,  the  CMM,  which  is  a  system  of  three 
prismatic  joints,  establishes  a  unique  reference  frame  even 
though  it  seems  quite  obvious  at  first  glance. 

Once  a  reference  frame  is  established,  the  end 
effector  frame  is  analyzed.  Most  non-constraining  type 
measurement  systems  measure  points  on  the  end  effector  in 
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Figure  57.  Measurement  System  Kinematic  Model  Development 


either  spherical  or  cartesian  coordinates  or  can  be  modelled 
in  this  fashion.  It  is  not  necessary  for  the  point  to  be  fully 
defined  to  be  modelled  as  such  as  in  the  case  of  the 
theodolite.  In  this  example,  the  azimuth  and  elevation  angles 
are  known  but  the  distance  between  the  reference  frame  and  the 
point  is  unknown.  If  additional  points  or  other  geometric 
factors  are  measured  for  each  set  of  joint  variables,  then 
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full  pose  or  at  least  some  aspects  of  orientation  can  be 
identified . 

For  constraining  type  measurement  systems,  it  is 
usually  advantageous  to  analyze  the  end  effector  fixture 
first.  Analysis  of  the  degrees  of  freedom  of  the  end  effector 
fixture  at  an  arbitrary  point  will  begin  to  define  the  make-up 
of  .  For  example,  the  end  effector  fixture  for  the  Linear 
Slide  method,  which  is  essentially  the  x  axis  carriage  of  the 
CMM,  is  fixed  in  orientation  at  any  position  along  the  slide. 
In  the  Modified  Linear  Slide  method,  the  ball  joint  allows 
three  rotational  degrees  of  freedom.  The  degrees  of  freedom 
for  mechanical  joints  are  easily  identified  and  provide  much 
of  the  needed  information.  The  result  of  this  analysis  may  or 
may  not  define  a  fixed  point  or  location  in  space. 

The  next  step  is  to  analyze  end  effector  motion  as  the 
manipulator  is  varied  through  a  series  of  measurements.  Motion 
along  known  paths  or  surfaces  may  further  define  or  may 
begin  to  define  the  location  of  F*’.  For  example,  a  unique 
reference  point  was  established  by  the  plane  defined  by  end 
effector  motion  in  the  Planar  Motion  calibration  example. 

At  this  point  in  the  process,  additional  joints  or 
features  of  the  measurement  system  should  be  identified. 
Additional  joints  will  likely  establish  the  reference  frame 
origin.  For  example,  the  fixed  ball  joint  in  the  Ball  Bar 
method  or  the  funnel  throat  in  the  Wire  Potentiometer  method 
define  the  origin  of  F".  However,  this  point  would  already  be 
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established  in  the  Ball  Bar  method  by  considering  the 
spherical  surface  traced  out  by  end  effector  motion. 

Specific  or  unique  locations  of  each  frame  may  be 
defined  when  this  step  is  reached.  However,  as  in  the  case  of 
the  Linear  Slide  method,  no  unique  point  in  space  is  defined 
by  the  measurement  system  considered  alone  since  only  a 
direction  is  defined.  As  illustrated  in  the  Linear  Slide  model 
development,  either  or  must  be  defined  coincident  with 
either  the  manipulator  base  frame  or  the  last  defined  frame  of 
the  manipulator  kinematic  model.  When  this  assignment  is  made, 
additional  parameters  of  the  other  measurement  system  frame 
may  be  fixed  based  on  geometric  relationships. 

The  final  step  involves  incorporating  "instrumented" 
information  provided  by  the  measurement  system  into  the  model. 
For  example,  if  a  ball  joint  is  "instrumented"  to  provide 
elevation  angle  above  some  prescribed  zero,  then  a  specific 
axis  is  specified  at  the  ball  joint  origin. 

The  large  scope  of  measurement  systems  and  devices 
capable  of  being  used  in  manipulator  calibration  defies  a  more 
rigorous  algorithm  for  model  development.  However,  the 
preceding  approach  does  provide  a  sound  approach  which  will 
result  in  a  properly  defined  model. 
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V.  CONCLUSIONS 


A.  EXPERIMENTAL  RESULTS 

-  A  PUMA  560  manipulator  can  be  calibrated  using  either  one 
of  the  three  methods  with  a  resulting  accuracy  which 
approaches  or  eguals  the  repeatability  of  0.3  mm. 

-  The  Coordinate  Measuring  Machine  calibration  method  will 
identify  the  "complete"  30  parameter  kinematic  model  with 
a  resulting  accuracy  of  0.3  mm. 

-  The  Modified  Linear  Slide  method  will  identify  26 
parameters  with  a  resulting  accuracy  of  0.74  mm. 

-  The  Wire  Potentiometer  will  identify  24  parameters  with  a 
resulting  accuracy  of  0.49  mm. 

-  The  Modified  Linear  Slide  method  did  result  in  a  more 
accurate  calibration,  0.74  mm,  than  the  Linear  Slide 
method,  0.9  mm,  which  emphasizes  the  need  for  large  joint 
variable  excitation. 

-  The  lower  calibration  accuracy  achieved  with  the  Linear 
Slide  Method  as  compared  to  the  less  accurate  Wire 
Potentiometer  supports  the  theory  that  "random"  poses  are 
more  desirable  than  poses  restricted  to  paths  or 
trajectories  regardless  of  joint  excitation. 

-  The  Wire  Potentiometer  method  provides  an  accurate, 
inexpensive,  portable  calibration  device  which  is  easily 
automated  and  capable  of  rapid  calibration  of  a  large 
class  of  manipulators. 

B.  MEASUREMENT  SYSTEMS  WITHIN  CLOSED  CHAIN  KINEMATIC  MODELS 

-  A  manipulator  kinematic  model  can  be  developed  which  will 
exhibit  no  parameter  dependancies  within  transformations 
to  external  reference  frames 

-  The  number  of  identifiable  parameters  of  a  typical  serial 
link  manipulator  in  a  closed  chain  kinematic  model  is 
given  by  Equation  52  and  for  the  PUMA  560  the  number  of 
parameters  is  18. 

-  The  process  of  developing  closed  chain  kinematic  models 
with  embedded  measurement  systems  for  parameter 
identification  can  be  divided  into  three  separate  tasks: 
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Develop  the  "manipulator  kinematic  model";  Develop  the 
"measurement  system  kinematic  model";  Link  the  two  models 
using  only  the  required  parameters  of  a  RPYT 
transformation. 

No  comprehensive  algorithm  for  measurement  system 
kinematic  model  development  exists  due  to  the  wide  variety 
of  measurement  systems  which  can  be  employed.  However, 
some  useful  guidelines  may  be  employed. 
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APPENDIX  A 


PROGRAM  BLINSC 


C  This  proqrai  generates  sets  of  joint  angles  for  the  Puna  lanipulator 
C  ar*.  It  assuies  that  the  tool  fra«e  of  the  lanipulator  is 
C  constrained  to  love  in  the  positive  i  direction  only.  The 
C  tool  is  constrained  by  a  ball  joint  lounted  to  a  sliding  linear  scale. 
C  The  values  along  the  x  direction  are  generated  by  a  random  number 
C  generator. 

INTEGER  LDFJAC,  H,  N,  OBS,  NOBS 
PARAMETER  (LDrJAC=3,  H=LDFJAC,  N-6| 

REAL*8  fiO,  thO,  siO,  pxO,  pvO,  p:0 
REAL*8  DTI,  DT2,  DT3,  DT4,  DT5 
REAL*8  DDl,  DD2,  DD3,  DD4,  DD5 
REAL*8  AAl,  AA2,  AA3,  AA4,  AA5 
REAL*8  ALl,  AL2,  AL3,  AL4,  ALB 
R£AL*8  BLl,  BL2,  BL3,  BL4,  BL5 
REAL*8  DF6,  FI6,  TH6,  SI6,  PX6,  PY6,  P;6 

REAL*8  RH1,RN2,RN3,RN4,RN5,RN6 
REAL+8  RN7,RN8,RN9,RN10,RN11,RN12 
REAL*8  RN13,RN14,RN15,RN16,RN17,RH18 

INTEGER  INFER, IER,IOPT,NSIG,HAXFN 

REAL*8  FJAC(LDFJAC,N),  XJTJ((N+l)*N/2),  XJAC(LDFJAC,N) 

RIAL*8  PARH(4),  F(LDFJAC),  WORK( (5*N)+(2*H)+((N*l)*N/2) ) 

REAL*8  X(N) 

REAL*8  HAGNX,HAGN1 

EXTERNAL  Pm_ARM 

INTEGER  I,  J,  K,  NOU 

REAL*8  TDES(3),  T(4,4),  SCALE,  DANGLE,  DLENTH,  NUM 

COMMON  /PDATA/  TDES,  DANGLE,  DLENTB,  T 
COMMON  /KIN/  FT0,TH0,SI0,PX0,PY0,P20 
&  DT1,DT2,DT3,DT4,DT5, 

&  AL1,AL2,AL3,AL4,AL5, 

&  AA1,AA2,AA3,AA4,A>.5, 

&  DD1,DD2,DD3,DD4,DD5, 

i  BL1,BL2,BL3,BL4,BL5, 

S  DF6,TB6,SI6,PX6,PY6,PZ6 
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C  Initialize  data  variables 


OBS=0 


C  Open  data  files  for  input  and  output 

OPEH  (10,  liAHE= 'PDHi.-POS.DAT',  STATUS='NrW' ) 
OPEN  (9,  NAHE=' INPUT. DAT',  STATUS-'OLD') 

C  Read  input  kineiatic  data 


READ 

(9,*) 

RE.JD 

(9,*) 

fio,tho,sio,pxo,pyo,pzo 

READ 

(9,*) 

Dn,DDl,AAl,ALl,BLl 

READ 

(9,*) 

DT2,DD2,AA2,AL2,BL2 

READ 

(9,*) 

DT3,DD3,AA3,AL3,BL3 

RE.ID 

(9,*) 

DT4,DD4,AJ.4,AL4,BL4 

RL‘D 

(9,*i 

DT5,DD5,A.A5,AL4,BL5 

RLAD 

(9,*1 

READ 

(9,*) 

DF6,TE6,SI6,PX6,PY6,P26 

READ 

(9,*) 

RE.AD 

(9,*) 

NOBS ,  NOU ,  DANGLE ,  DLENTH ,  MA.GNV ,  HAGKL 

CLOSE  (9) 

C  Adjust  noainal  values 

FI0=FI0tDANGLE 

TH0=TH0*DANGLE 

S10=0.0 

PX0=PX0+DLENTH 

PY0=PY0+DLENTH 

pzo=p:o^dlenth 


DT1=0.0 

DT2=DT2+DANGLE 

DT3=DT3+DANGLE 

DT4=DT4+DANGLE 

DT5=DT5+DANGLE 

AL1=AL1+DANGLE 

AL2=AL2+DANGLE 

AL3=AL3+DANGLE 

AL4=AL4+DANGLE 

AL5=AL5+DAHGLE 

AA1=AA1+DLENTH 

AA2=AA2+DLEJrrH 

AA3=AA3+DLENTH 

AA4-AA4+DLENTB 

AA5=AA5+DLEirrH 
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DD1=0.0 

DD2=0.0 

DD3-DD3-DLEinE 

DD4=DD4+DLENTE 

DD5=DD5tDLENTH 

BL1=BL1 

BL2=BL2+DABGLE 

BL3=BL3 

BL4=BL4 

BL5=BL5 

DF6=DF6*DAJGLE 

TH6=TB6 

SI6=SI6 

PX6=PX6+DLENTH 

PY6=PY6 

P26=PZ6+DLENTH 
C  Get  randoB  nuiber  seed 

WRITE  (6,*)  'Type  in  a  6-digit  randoo  nuiiber  seed' 
READ  (5,*)  ISEED 

C  Start  of  tain  loop 

1010  OBS=OBS-1 

C  Set  initial  values  of  joint  variables 

X(l)=70.0 

X(2)=0.0 

X(3)=90.0 

X(4)=0.0 

X(5)=50.0 

X(6)=90.0 

C  Get  randoi  slide  lengths 

1000  CALL  RANDOM  (ISEED, HUM i 
JIDH=NDH*940.0 

C  Establish  desired  tool  position 

TDES(1)=  HUM 
TDES(2)=  0.0 
TDES(3)=  0.0 

C  Call  IHSL  ZXSSQ  for  inverse  kineiatic  solution 
HSIGM 
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EPS=0.0 

DELTA-0.0 

MAXFN-500 

IOPT-1 

nJAC=LDFJAC 

CALL  2 XSSQ  ( PtJH.A_ARH ,  H ,  H ,  NSIG ,  EPS ,  DELTA , M.AXFK ,  lOPT ,  PAW* ,  X , 
&  SSQ,  F,  XJAC,  nJAC,XJTJ, WORK,  INFER,  lER) 

C  Check  for  singularities 

IF  (SSQ  .GT.  0.00001)  GOTO  1000 

C  Print  results  to  2  decital  places 

WRITE  (6,*)  OBS,SSQ 

C  Generate  the  randoB  noise 

CALL  RANDOM  (ISEED,RN1) 

CALL  RANDOM  (ISEED,RN2) 

CALL  RANDOM  (ISEED,RN3l 
CALL  RANDOM  (ISEED,RK4) 

CALL  RANDOM  (ISEED,RN5) 

CALL  RANDOM  (ISEED,RN6) 

CALL  RANDOM  (ISEED,RN"I 
CALL  RANDOM  (ISEED,RN3l 
CALL  RANDOM  (ISEED,RN9) 


RNl  = 

MAGNX 

* 

(2.0 

RNl  - 

1.0) 

RN2  = 

MAGNX 

* 

(2.0 

* 

RN2  • 

1.0) 

RN3  = 

MAGNX 

* 

(2.0 

* 

RN3  - 

1.0) 

RN4  = 

HAGNl 

« 

(2.0 

* 

RN4  - 

1.0) 

RN5  - 

MAGNl 

* 

(2.0 

* 

RN5  - 

1.0) 

RH6  = 

MAGNl 

* 

(2.0 

* 

RN6  - 

1.0) 

RN7  = 

MAGNl 

* 

(2.0 

* 

RN7  - 

1.0) 

RN8  = 

MAGNl 

* 

(2.0 

* 

RN8  - 

l.Oi 

RN9  = 

MAGNl 

* 

(2.0 

* 

RN9  - 

1.0) 

C  Iniect  randoi  noise 

X(l)  =  X(l)  +  RN4 

X(2)  =  X(2)  +  RN5 

X{3)  -  X(3)  +  RN6 

X(4)  -  X(4)  +  RIT 

X(5)  -  X(5)  +  RN8 

X(6)  =  X(6)  +  RN9 

TDESfl)-TDES(l)*RNl 

TDES(2)-TDES(2I*RN2 


143 


TDES(3t=TDESi3)+RIi3 

WRITE  (10,*)  X(l),X(2l,X(3!,)((4),X(5),X(6) 

WRITE  (10,*)  TDES(l),TDESi2),TDESi3) 

WRITE  (10,*) 

C  Continue  for  other  slide  positions 
IF  (OBS  .LT.  NOBSl  GOTO  1010 
CLOSE  (10 1 

END 

SDBROCTINE  PLm.ARH  (X,H,N,F) 

C  This  subroutine  calculates  the  non-linear  function  for  the  use  of 
C  the  IHSL  routine  2XSSQ.  It  is  the  inverse  kineiatic  solution  for 
C  the  PUH>.  lanipuiator. 

INTEGER  H,  N 
REAL*8  X(N),  Fl'Hl 

INTEGER  II,  JJ 

RL4L*8  FIO,  THO,  SIO,  PXO,  PYO,  PZO 
R£AL*8  DTI,  012,  DT3,  DT4,  DT5 
REAL*8  DDl,  DD2,  DD3,  DD4,  DD5 
REAL*8  AAl,  AA2,  AA3,  AA4,  AA5 
REAL*8  >11,  >12,  >13,  AL4,  >15 
RE>1*8  BLl,  BL2,  BL3,  BL4,  BL5 
REAL*8  DF6,  FI6,  TH6,  SI6,  PX6,  PY6,  PZO 

REAL*8  THl,  TH2,  TH3,  TH4,  TH5 

REAL*8  TO(4,4),  Tl(4,4),  T2l4,4),  T3(4,4|,  T4(4,4) 

REAL*8  T5(4,4),  T6(4,4),  trpy(4,4),  txy:(4,4) 

REM.*8  TIHAT(4,4),  T(4,4),  td(4,4) 

INTEGER  I,  J,  K 

REAL*8  TDES(3),  DANGLE,  DLENTB,  SCALE 

COWON  /PDATA/  TDES,  DANGLE,  DLENTH,  T 
COWON  /KIN/  FIO, THO, SIO, PXO, PYO, P20 
4  DT1,DT2,DT3,DT4,DT5, 

4  AL1,AL2,AL3,AL4,AL5, 

4  AA1,AA2,AA3,AA4,AA5, 

4  DD1,DD2,DD3,DD4,DD5, 

4  BL1,BL2,BL3,BL4,BL5, 

4  DF6,TH6,SI6,PX6,PY6,P26 
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C  Initiali:e  the  TIRi.T  latrix  to  an  I  latrix: 


DATA  TIHA.T/l, 0,0, 0,0, 1,0, 0,0, 0,1, 0,0, 0,0,1 


SCALE^IOO.O 

C  Initialize  the  T  latrix  to  an  I  latrix 

DO  II  =  1,4 
DO  JJ  =  1,4 

T(II,JJ)  =  TIHA.T(II,JJi 
ENDK 
ENDX 

C  Manipulator  joint  angles 

THl  =  DTI  +  X(l') 

TH2  =  DT2  +  X(2) 

TH3  =  DT3  t  Xi31 

TH4  =  DT4  +  Xl4) 

TE5  =  DT6  +  X(5i 

ri6  =  DF6  +  Xl6l 

C  Coipute  the  T  latrices,  T1  thru  T6: 

CALL  T3RPY  (FI0,TH0,SI0,TRPY) 

CALL  T3XY2  (PX0,py0,P20,TXY: I 
CALL  HATHULC  (T0,TRPY,TXY: ) 

CALL  TRANSFORM  (  ALl,  AA.l,  DDl,  THl,  BLl,  Ti  ) 

CALL  TRANSFORM  (  AL2,  AA.2,  DD2,  TH2,  BL2,  T2  ) 

CALL  TRANSFORM  (  AL3,  AA3,  DD3,  TH3,  BL3,  T3  ) 

CALL  TRANSFORM  (  AL4,  AA4,  DD4,  TH4,  BL4,  T4  ) 

CALL  TRANSFORM  (  AL5,  AA5,  DD5,  TH5,  BL5,  T5  ) 

CALL  T3RPY  (  FI6,  TH6,  SI6,  TRPY  ) 

CALL  T3XY2  (  PX6,  PY6,  P26,  TXYI  ) 

CALL  MA.TMULC  (  T6,  TRPY,  TXYZ  1 

C  Coipute  the  overall  transfonation,  T: 

CALL  MATMULA  (  T,  TO  ) 

CALL  MATMULA  (  T,  Tl  ) 

CALL  MATMULA  (  T,  T2  ) 

CALL  MATMULA  (  T,  T3  ) 

CALL  MATMULA  (  T,  T4  ) 

CALL  MATMULA  (  T,  T5  ) 

CALL  MATMULA  (  T,  T6  ) 

C  Calculate  the  function  F 
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F(l)=T(l,4)-TDE£lll 

F(2l=T(2,4) 

Fi3i=Ti3,4) 

C  Calculate  residual  and  write  to  screen 

SUM  =  0.0 
DO  i=l,3 
XSSQ=SD«'F(I) 

ENDDC' 

»rlITE  (6,*)  XSSO 

RHURN 

END 

Q  ****Hi*it****tHi*t*ii*Hi*H,i,i,ii,t****H*ii******t***1tiii***Ht1i*i 

SUBROUTINE  RANDOM  (X,!) 

C  This  subroutine  generates  randoo  nuubers  in  the  range  0-1 
C  using  a  supplied  seed  r,  the  returned  randoa  nuiber  being  i. 

RE.1L  FH,  FX,  : 

INTEGER  A,  X,  I,  H 
DATA  II 


IF  (  I  ,EQ.  0  1  GO  TO  1000 
1=0 

H=  2  20 

FH=  H 

A=  2**10  -  3 
1000  X=  MODi  A*X  ,H) 

FX=  X 
Z=  FX  '  FM 

RETURN 

END 
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APPENDIX  B 


PROGRAM  BID6 

C  Robot  Identification  using  the  Non-linear  Least  Squares  Bethod  for  the  Bodified  linear  slide  sethod. 
C  SiBulation  data  is  read  for  the  PCMA  Banipulator  froB  the  data  file  PCHA-POS.DAT 

C  Change  paraBeter  LDFJAC  to  change  the  nuEber  of  observations, 

C  set  LDFJAC  =  6  *  Nuiber  of  observations 

INTEGER  LDFJAC,  MM,  H,  NN,  N,  HSIG,  HAXFN,  lOPI,  IXJAC,  INFER,  lER 
PARAMETER  (LDFJAC-3*100,  MM-LDFJAC,  NN-26) 

REAL*8  FJAC(LDFJAC,NN),  XJTJ( (KN"1)*NN  2) 

REAL*8  PARM(4),  P(LDFJAC),  »ORK((5*NN)+(2*HM)+((NN+l)*NN '2) I 
REAL*8  X(NN) 

EXTERNAL  PLm_ARH 

RLAL*8  DANGLE,  DLENTH,  TQ,  DQ,  EPS,  DELTA,  SSQ 
R£AL*8  SQERRl,  SQERE2 

REAL*8  FI0,TH0,SI0,PX0,Py0,P:0 
REAL*8  DTI,  DT2,  DT3,  DT4,  DT5 
R£AL*8  DDl,  DD2,  DD3,  DD4,  DD5 
R£AL*8  AAl,  AA2,  AA3,  AA4,  AA.5 
RLAL*8  ALl,  AL2,  AL3,  AL4,  AL5 
R£AL*8  BLl,  BL2,  BL3,  BL4,  BL5 
R£AL*8  DF6,  TH6,  SI6,  PX6,  PY6,  PZe,  FI6 

INTEGER  I,  J,  K,  HOBS,  MAXNOBS 
PARAMETER  (MAXN0BSO60) 

REAL*8  TETKHAXNOBS),  TET2( MAXNOBS),  TET3( MAXNOBS) 

REAL* 8  TET4( MAXNOBS),  TET5 I MAXNOBS),  TET6( MAXNOBS) 

REAL*8  TM(3,KAXHOBS),  SCALE 
COMMON  /PDATA/  NOBS,  TM,  SCALE, 

STETl,  TET2,  TET3,  TET4,  TCTS,  TET6 

C  Open  data  files  for  inputs  and  results 

OPEN  (8,  NAME- 'RESULT. DAT',  STATDS^'NrW' ) 

OPEN  (9,  NAME-'PUMA-POS.DAT',  STATUS^'OLD') 

OPEN  ( 10, NAME= 'IHPDT.DAT',  STATCS='OLD') 

c  Read  input  paraBeters 

READ  (10,*) 

READ  (10,*)  FIO,THO,SIO,PXO,PYO,P20 
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RDD  (10,*)  DT1,DD1,A>.1,AL1,BL1 
READ  (10,*)  DT2,DD2,AA2,AL2,BL2 
READ  (10,*)  DT3,DD3,AA3,AL3,BL3 
READ  (10,*)  DT4,DD4,AA4,AL4,BL4 
READ  (10,*)  DT5,DD5,AA5,AL5,BL5 
READ  (10,*) 

READ  (10,*)  DF6,TH6,SI6,PX6,PY6,P26 
READ  (10,*) 

READ  (10,*)  NOBS,N, DANGLE, DLENTH,HAGNX,NAGK1 
CLOSE  (10) 

C  Initialize  data  variables 

X(1)=FI0 

X(2)=TB0 

X(3)=PX0 

X(4)=PY0 

X(5)=PZ0 


x,6)=Ai.l 

X(7)=AL1 

X(8)=DT2 

X(9)=A^2 

X(10)=AL2 

X(11)=BL2 

X(12)=DT3 

X(13)=DD3 

X(14)=A.i3 

X(15)=AL3 

X(16)=DT4 

X(17)=DD4 

X(18)=AA4 

X(19)=AL4 

X(20)=DT5 

X(21)=DD5 

X(22I=AA5 

X(23)=AL5 

X(24)=DF6 

X(25)=PX6 

X(26)=P26 

C  Read  simulated  joint  data  and  tool  pose 
DO  J  =  1,  HOBS 

READ  (9,*)  TETl(J),  TET2(J),  TET3(J),  TET4(J),  TET5(J1,  TET6(Jj 
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RLU)  (9,*)  TH(1,J),  TM(2,J1,  m;3,Jl 
READ  (9,*) 

ENDDO 
CLOSE  (9) 

C  Initialize  scale  for  the  angular  rows  of  the  Jacobian 
SCALE=100.0 

C  Call  IHSL  routine  for  non-linear  identification 

HSIGM 

EPS=0.0 

DELTA=0.0 

MAXF1I=1500 

IOPT-1 

IXJAC^LDFJAC 

H=3*N0BS 


CALL  I XSSQ  ( POULARH ,  H ,  N ,  NSIG ,  EPS ,  DELTA ,  KAXFK ,  lOPT , 

&  PARH,X,SSQ,F,FJAC,IXJAC,XJTJ, WORK, INFER, lER) 

C  Save  results  to  data  file 


WRITE  (8,*) 

WRITE  (8,*)  'FIO,  THO,  SIO,  PXO,  PYO,  PZO' 

WRITE  (8,888)  X(l),  X(2),  0,0,  X(3),  x(4),  x(5) 
WRITE  18, *j 

WRITE  (8,*)  'DTI,  DDl,  W.l,  ALl,  BLl' 

WRITE  (8,888)  0.0,  0,0,  X(6),  X(7),  0.0 
WRITE  (8,*) 

WRITE  (8,*)  'DT2,  DD2,  AA2,  AL2,  BL2' 

WRITE  (8,888)  X(8),  0.0,  X(9),  X(10),  X(ll) 

WRITE  (8,*) 

WRITE  (8,*)  'DT3,  DD3,  AA3,  AL3,  BL3' 

WRITE  (8,888)  X(12),  X(13),  X(14),  X(15l,  0.0 
WRITE  (8,*) 

WRITE  (8,*)  'DT4,  DD4,  AA4,  AL4,  BL4' 

WRITE  (8,888)  X(16),  X(17),  X(18),  X(19),  0.0 
WRITE  (8,*) 

WRITE  (8,*)  'DT5,  DD5,  AA5,  AL5,  BL5' 

WRITE  (8,888)  X(20),  X(21),  X(22),  X(23),  0.0 
WRITE  (8,*) 

WRITE  (8,*)  'DF6,  TH6,  SI6,  PX6,  m,  P:6' 

WRITE  (8,889)  X(24),  0.0,  0.0,  X(25),  0.0,  X(26) 

888  FORMAT  (  5F12.5  ) 

889  FORMAT  (  6F12.5  ) 

C  Calculate  root  lean  square  error  in  identification 
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TQ  =  D>JJGLE 
DQ  =  DLE3rrE 


C  Error  in  identification  (angular  paraneters) 

SQERRl  = 

&  (FIO+TQ-X(l))**2  +(SI0+Ti}-X(2))**2 
&  +(DT3+TQ-X(12))**2  +(DT4+TQ-X(16))**2  +(DT5+TQ-X(20) 1**2 
&  +(AL1+TQ-X(8))**2  +(AL2+TQ-X(11))**2 

k  +(AL3+TQ-X(15))**2  +(AL4+T0-X(19) )**2  +(AL6+T0-X(23) )**2 
k  +(BL2+TQ-X(11))**2  +(DT2+TQ-X(8))**2 
k  +(DF6+TQ-X(25))**2 
SQERRl  =  DSQRTl  SQERRl /13  ) 

C  Error  in  identification  (length  paraieters) 

SQERR2  = 

k  (PXO+DQ-X(3))**2  +(PYO+DQ-X(4)(**2  +(P:OfI>5-X(5))**2 
&  +(AA1+DQ-X(6))**2  +(AA2+DQ-X(9))**2 

k  +(>13»DQ-X(14))**2  +(AA4+DQ-X(18))**2  +(AA5+DQ-X(22))**2 
k  +(DD3+DQ-X(14))**2  +(DD4+DQ-X(18) )**2  +(DD5+DQ-X(22) )**2 
&  +(PX6*DQ-XI25))**2  +(PZ6+DQ-X(26))**2 
SQERR2  =  DSQRT(  SQERR2/13  ) 

WRITE  (8,*) 

WRITE  (8,*)  'RMS  PARKS  (LENGTH),  RMS  PARKS  (ANGLE)' 

WRITE  (8,*)  SQERR2,  SQERRl 

WRITE  (6,*)  'RKS  PARKS  (LENGTH),  RKS  PARKS  (ANGLE)' 

WRITE  (6,*)  SQERR2, SQERRl 


WRITE  (8,*) 

WRITE  (8,*) 

'INFER, 

IER,NOBS,NSIG' 

WRITE  (8,*) 

INFER, 

IER,NOBS,HSIG 

WRITE  (6,*) 

'INFER, 

IER,NOBS,NSIG' 

WRITE  (6,*) 

INFER, 

IER,HOBS,HSIG 

WRITE  (S,*) 

CLOSE  (8) 

END 


SDBROOTINE  PUKA.ARK  (X,  H,  N,  F) 

C  This  subroutine  calculates  the  non-linear  function  for  the  use  of 
C  the  IMSL  routine  DCNLSF.  It  is  the  forward  kineiatic  solution  for 
C  the  PUKA  lanipulator. 
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INTEGER  H,  N 
RLiL*8  X(N;,  F(H| 

INTEGER  II,  JJ 

REAL*8  FIO,THO,SIO,PXO,PYO,P:0 
RE>.L*8  DTI,  DT2,  DIB,  DT4,  DT6 
REAL*8  DDl,  DD2,  DD3,  DD4,  DDE 
REAL*8  AAl,  L:2,  A>.3,  M4,  LKb 
REAL*8  All,  AL2,  ALB,  AL4,  ALB 
REAL*8  BLl,  BL2,  BL3,  BL4,  BL5 
R£AL*8  FI6,  TH6,  SI6,  PX6,  PY6,  P26,  DF6 

REAI*8  TEl,  TH2,  TH3,  TH4,  TEE 
RLAL*8  TO|4,4),  Tl(4,4),  T2(4,4),  T3(4,4),  T4(4,4) 
RD1*8  T5(4,4l,  T6(4,4),  TRPY(4,4h  TXY:(4,41 
RLiL*8  TIM.‘T(4,4),  T(4,4| 

RE>I*8  TIN';(4,4),  THJ(4,4|,  TDELTAI4,4) 

INTEGER  I,  J,  K,  NOBS,  HA.XNOBS 
PARAMETER  (HAXNOBS=360i 

REAL*8  TETK HA.XNOBS),  TET2 ( HA.XNOBS ) ,  TET3(MA.XNOBS) 
RE.AL*8  TET4(HAXNOBS),  TET5(HAXNOBS),  TET6(HAXHOBSi 
R£A.L*8  TH(3,HAXNOBS),  SCAIE 
COMMON  /PDA.TA'  NOBS,  TH,  SCALE, 

&  TETl,  Tn2,  TET3,  TET4,  TET5,  TET6 

C  Initialize  the  TIHAT  latrix  to  an  I  latrix: 

DATA  TIMA.T;1, 0,0, 0,0, 1,0, 0,0, 0,1, 0,0, 0,0,1 ' 

C  Set  paraieters  for  the  lanipulator: 

FI0=X(1) 

TH0=X|2l 

SI0=0.0 

PX0=X(3) 

PY0=X(4l 

p:o=x(5i 


DT1=0.0 

DD1=0.U 

AAl=x(6) 

AL1-X(7) 

BL1=0.0 

DT2-X(8l 

DD2=0.0 

AA2=X(9) 

AL2^X(10) 

BL2=Xllll 

DT3=X(12) 
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DD3=X(13) 

AA3=X(14) 

AL3=X(151 

BL3=0.0 

DT4=X(16) 

DD4=X(17) 

AA4-X(18i 

AL4^X(19'1 

BL4^C.O 

DT5=X(20) 

DD5=X(21) 

AA5^X(22'I 

AL5=X(23l 

BL5=0.0 


DF6=Xi24l 

TH6^0.0 

SI6:=0.0 

PX6=X(25l 

PY6=0.0 

PZ6=X(2ei 


C  Loop  NOBS  tiaes 
F  -  0 

DO  J  =  1,  NOBS 

C  Initialixe  the  T  latrix  to  an  I  aatrix 

DO  II  =  1,4 
DO  JJ  =  1,4 

T(II,JJ)  ==  TIH3.T(II,JJ) 

ENDDO 

EJfDDO 

C  Hanipulator  joint  angles 

THl  =  DTI  +  TETKJ) 

TH2  =  DT2  +  TET2(J) 

TH3  =  DT3  +  TET3(J) 

TH4  =  DT4  +  TET4(J) 

TH5  =  DT5  +  TET5(J) 

FI6  =  DF6  +  TET6(J) 

C  Coipute  the  T  latrices,  T1  thru  T6: 


CALL  T3RPY  (  FIO,  THO,  SIO,  TRPYi 
CALL  T3XYZ  (  PXO,  PYO,  PZO,  TXYZ) 
CALL  HATHULC  (  TO,  TRPY,  TXYZi 
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CALL  TR>JiSFOPJ<  (  ALL,  AA.l,  DDL,  TEl,  BLl,  T1  ) 

CALL  TRANSFORM  (  AL2,  W.2,  DD2,  TH2,  BL2,  T2  ) 

CALL  TRANSFORM  (  AL3,  AA.3,  DD3,  TH3,  BL3,  T3  ) 

CALL  TRANSFORM  (  AL4,  AA.4,  DD4,  TH4,  BL4,  T4  ) 

CALL  TR.iJiSFORH  (  AL5,  AA.5,  DD5,  TH5,  BL5,  T5  ) 


CALL  T3RPY  (  FI6,  TH6,  SI6,  TRFY  ) 
CALL  T3>;y;  (  p):e,  m,  pze,  txv;  ) 
CALL  HATHULC  (T6,  TRPi,  TXYZ  ) 


C  Conpute  the  overall  transforaation,  T: 

OIL  MATHULi.  (  T,  TO  ) 

CALL  HiTHCLi  (  T,  II  ) 

CALL  MATHllA  (  T,  T2  ) 

CALL  M.JTKUL1  (  T,  T3  1 
CALL  MATHULl  (  T,  T4  ) 

CALL  HATHiLA.  (  T,  T5  ) 

CALL  HATHULl  (  T,  To  ) 

C  Get  the  "T-ieasured"  aatrix  for  this  observation: 


DO  II  =  1,3 
DO  JJ  =  1,4 

THJ(II,JJ)  =  TH(II,JJ,J1 

mx 

ENDDO 


THJi4,l)  =  C.O 
TMJ(4,2)  =  0.0 
THJl4,3)  =  0.0 
THJ(4,4)  =  1.0 

C  Calculate  the  functio.ns  F 

K  K  M 

F(K)  =  T(l,4)-TH(l,Jl 
K  =  K  t  1 

FiKt  =  T(2,4)-TH(2,J) 

K  =  K  +  1 

F(K)  =  T(3,4!-TH(3,J) 

C  End  the  do-loop  for  counter  J 
ENDDO 

C  Write  RMS  error  in  F 

XSSQ=0.0 
DO  II=l,3*NOBS 
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XSSQ=XSSQ-Flir)*F(ir! 

ENDDO 

XER=SQRT(XSSQi 
WRITE (6,*)  XER 


RETURN 

END 
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APPENDIX  C 


C  t*ii**iiiHtii*iti**itit**H!ii****itiHt******ii*it***it**t***1ti*i*i*t**i*i**Hi** 

PROGRAM  VERIFY 

C  This  program  generates  the  six-dof  pose  error  for  the  PUH.I  manipulator. 

C  It  contains  the  identified  calibration  parameters  and  the  exact  parameter. 
C  It  uses  a  data  file  of  verification  joint  angle  sets  POSEVER.DAT,  and  the 
C  file  RESDLT.DAT  from  the  program  ID6. 

INTEGER  I,  J,  K,  NPOSES,  N 
REAL*8  DANGLE,  DLENTE 

REAL*8  DT(5),DD(5),AA(5),AL(5),BL(5l,HD.S(6) 

REAL*8  EDT(5),EDD(5),EAA(5),EAL(5),EBL(5),EHEAS(6) 

RLAL*8  EDF6 , EFI6 , ETE6 ,ES16 , EPX6 , EPY6 , EPZ6 
REAL*8  THETA (1000,6),  TDELTA(4,4l 
REA1*8  TO(4,4),  Tl(4,4),  T2l4,4),  T3i4,4) 

REAL*8  T4(4,4),  T5(4,4),  T6(4,4j,  TRPY(4,4),  TXYZ(4,4) 

R£AL*S  TIMAT(4,4j,  T(4,4),  ET(4,4) 

REAL*8  DTI,  DT2,  DT3,  DT4,  DT5 

REAL*8  DDl,  DD2,  DD3,  DD4,  DD5 

REAL*8  AAl,  AA2,  AA3,  AA4,  AA.5 

RD.L*8  ALl,  AL2,  AL3,  AL4,  AL5 

REAL*8  BLl,  BL2,  BL3,  BL4,  BL5 

R£A.L*8  DF6,  FI6,  TE6,  SI6,  PXo,  PY6,  p;6 
RLAL*8  XW,  YK,  ZK 
COMMON  TIMAT,THETA 

C  Initialize  the  TIMAT  matrix  to  an  I  matrix: 

DATA  TIM.AT/1, 0,0, 0,0, 1,0, 0,0, 0,1, 0,0, 0,0,1, ' 

C  Open  data  file 

OPEN  (9,  Nm='POSEVEE.DAT',STATL-S='OLD') 

OPEN  (10,  NAME= 'INPCT.DAT',  STATUS^'OLD' I 
OPEN  (11,  NAME= 'RESULT. DAT',  STATUS='OLD' ) 

C  Read  input  parameters 

READ  (10,*) 

READ  (10,*)  HLAS(l),HEAD(2),HEAS(3),MEAS(4),HEAS(5),MEAS(6l 
READ  (10,*)  DTI, DDl, AAl, ALl, BLl 
READ  (10,*)  DT2,DD2,A.A2,A12,BL2 
READ  (10,*)  DT3,DD3,AA3,AL3,BL3 
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RE-iJ)  (10, *1  DT4,DD4,AA4,AL4,BL4 
READ  (10,*)  DT5,DD5,AA5,AL5,BL5 
READ  (10,* 

READ  (10,*)  DF6,TH6,SI6,PX6,Py6,P;6 
READ  (10,*) 

READ  (10,*)  NOBS,R,D.ARGLE,DLEHTB,HAGNX,MA.GKL 
CLOSE  (10) 


C  Read  in  joint  angle  sets  for  verification  poses 

NPOSES^NOBS 

DO  M,NP0SES 
R£AD(9,*) 

READ  ( 9 ,  * )  THETA  (1,1),  THETA  (1,2),  THETA  (1,3) ,  THETA.  (1,4), 
&  THETA(I,5),THETA(I,6) 

EhDDO 
CLOSE) 9 1 

C  Set  exact  link  parameters  for  tPe  manipulator: 

DO  I:=2,5 

DT(I)=DTlI)tD.AHGLE 

ENDDC 

HEAS(1)=HEAS(1)*DLENTH 

MEAS(2)=HEAS(2)+DLENTH 

HE.iD(3l=MEAi(3l*DLENTH 

MEAS(4)=HEAS(4)+DLENTH 

HLAS(51=HLiD(5)tDLENTH 

MIAS(6)=HEAS(6)+DLENTB 

AL(1)-AL1*DAHGLE 

AL(2l=AL2+D.AHGLE 

AL(3)=AL3"DAHGLE 

AL(4)=AL4+DAHGLE 

AL(5)=AL5+DANGLE 

AA(1)  =  A.A1  +  DLENTH 

AA(2)  =  AA2  +  DLENTH 

AA(3)  =  AA3  +  DLENTH 

AA(4)  =  AA4  +  DLENTH 

AA(5)  =  AA5  +  DLENTH 

DD(1)  =  DDl 

DD(2)  =  DD2 

DD(3)  =  DD3  +  DLENTH 

DD(4)  =  DD4  +  DLENTH 

DDl 5)  -  DD5  +  DLENTH 
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BL(li  BLl 
BL(2l  =  BL2  +  DAMGLE 
B1 1 3 1  =  BL3 
BL(4)  =  BL4 
BLl 5 1  ^  BL4 

DF6  =  DF6  +  DANGLE 
THE  -  TE6 
SI6  =  SI6 

PXb  -  PX6  t  DLENTE 
PY6  =  PY6 

PZe  =  PZ6  +  DLLVTH 

C  Read  in  and  set  up  estiiated  paraaeter  table 

READill,*! 

REAJ)(11;*) 

READ  ( 11 ,  *  I  ( 1 ) ,  EHLAS  ( 2 1 ,  EME.AS  ( 3 ) 

DO  M,5 
READill,*! 

RE.ADi.ll,*) 

RLiJ)  ill,*)  EDTlI),EDD(I),EA.A(I),LAL(I),EBL(I) 
ENDDCi 


REiDlll,*! 

RDJ)(ll,*l 

RLADl 1 1 , * )  EDF6 , ETH6 , ESI6 , EPX6 , EPV6 , EPLc 
C  Main  loop  through  NPOSES  joint  angle  sets 
DO  K=l, NPOSES 

CALL  FKS  (K,HEAS,DT,AL,AA,DD,BL,FI6,TH6,SI6,PX6,PY6,PZ6,T) 
CALL  FKS  (K,EMEAS,EDT,EAL,EAA,EDD,EBL,EFI6,ETB6,ESI6,EPX6, 
4  EPY6,EPZ6,ET) 

C  Coapute  the  differential  tool  iatri>: 

CALL  HA.TSL'B(TDELTA,T,E.' 

c  Coapute  the  pose  errors 

POSERR-SQRT ( TDELTA (1,4) *  *2+TDELTA (2,4 ) **2*TDELTA ( 3 , 4 ) *  *2 ) 

ORERRl = ( TDELTA (3,2) -TDELTA ( 2 , 3 ) ) /2 

ORERR2- ( TDELTA ( 1 , 3 1 -TDELTA ( 3 , 1 ) ) / 2 

ORERR3= ( TDELTA ( 2 , 1 ) -TDELTA ( 1 , 2 ) ) /2 

ORERR-SQRT(ORERRl**2+ORERR2**2+ORERR3**2) 

c  Dpdate  total  error  counts 
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POSTERP-  ( POSERP.-  ( K-l)  *POSTEPJ? )  /K 
ORTERR^  ( OR£PJ?t  ( R-l )  *ORTERR  |  /K 

c  End  of  «ain  loop 

ENDDC 

UlITEie,*!  'Position  error,  orientation  error' 

WRITE16,*)  POSTERR,OP.TERR 

END 


C  ***irit**»***»*t*************************************************** 

SUBROUTINE  FKS  (N,HEAS,DT,AL,AA,DD,BL,DF6,TH6,SI6, 

i  px6,py6,p:6,Ti 

REAL*8  :0|4,4),  Ti(4,4l,  I2i4,4l,  T3l4,4) 

RLil*£  14(4,41,  T5i4,4),  T6i4,4),  TRPY(4,4),  TXY"(4,4) 

REAL*8  TIH.iT(4,4),  T(4,4),  dt(5),al(5),aa(5),dd(5),bl(5) 
REAi*8  THETA  ( 1000 , 6 1 , ANG ( 5 1  ,HLi^(  6 1 

COMMON  TIH.AT,THETA 

C  Initialize  the  T  latrix  to  an  I  latrix: 

DO  J=i,4 
DC'  1-1,4 

T  J,K)  =  TIHA.TiJ,K) 

ENT.>^ 

end:«: 


C  Set  up  the  joint  angles 
DC’  1=  ,5 

ANC-  I)-THETA'N,Il-DTlIl 
ENDDC' 


FI6=T-ETA(N,6!*DF6 

C  CoBpute  th’  T  latrices,  T1  thru  T6: 

CALL  T3RPY  (HLAS(l),HEASl2),HEAS(3i,T0) 

CALL  T3XY2  (HEAS(4),HEAS(5),MEAS(6),T0) 

CALL  HA.TMULC  (TO,TRPY,TXYZ ) 

CALL  TRANSFORM  (AL(1),AA(1),DB,'1),ANG(1),BL(1),T1) 
CALL  TRANSFORM  (AL(2),AA(2),DD(2),ANG(2),BL(2),T1) 
CALL  TRANSFORM  (AL(3),AA(3),DD(3),ANG(3),BL(3),TD 
CALL  TRANSFORM  IAL(4),AAl4),DD(4),ANGI4),BL(4l,Tl) 
CALL  TRANSFORM  (AL(5),AAl5),DD(5),ANG(5),BL(5l,Tl) 
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CALL  T3RPi  lFI6,TBe,SI6,TRPY  ) 
CAa  T3XYZ  (PX6,PY6,P:6,TXV;  ) 
OIL  HA.TH'JLC  (T6,TRPi ,TXY;  1 

C  Coipute  the  overall  transfonation,  T: 

OIL  Hi.TKLLA  I  T,  TO  ) 

CALL  MA.TMUO.  I  T,  11  ) 

CALL  K-XTHLO.  i  T,  12  ) 

CALL  MATHL’LA  (  1,  13  ) 

CALL  H.X1HULX  I  1,  14  i 
CALL  KA.THUO.  (  1,  15  l 
CALL  HA.TH’JLX  I  1,  16  1 


RETUPJi 

END 

c  ******************************************************************* 
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APPENDIX  D 


PROGRAM  WIRE 

C  This  prograi  generates  a  set  of  joint  angles  for  the  calibration 
C  of  the  POKA  lanipulator  using  a  wire  potentioieter  attached  to 
C  the  end  point  of  the  lanipulator. 

INTEGER  LDFJAC,  M,  N,  obs,  nobs 
PARAMETER  (LDFJAC=6,  H=LDFJAC,  N=6) 

REAL*8  DTI,  DT2,  DT3,  014,  DTS 
REAL*8  DDl,  DD2,  DD3,  DD4,  DD5 
REAL*8  AAl,  AA2,  AA3,  AA4,  A.A5 
RLAL*8  All,  AL2,  AL3,  AL4,  AL5 
REAL*8  BLl,  BL2,  BL3,  BL4,  BL5 
RLAL*8  DF6,  FI6,  TH6,  SI6,  PX6,  PY6,  PZe 
REAL*8  r»,  W,  ZK 

REAL*8  RUXY:(3),DXY2(3),XYZ,DALPEA,DBEIA,DGAJ01A.,ANrH 
REAL*8  PDIRCOS,HBIRCOS,PNGLBTWN,KNGLBTWi 

INTEGER  INFER, IER,IOPT,NSIG,MA.XFN 

REAL*8  FJAC(LDfJAC,Nl,  XJTJ('(N*II*K  71,  XJAC( LDFJAC, N I 

REAL*S  PARM(4),F(LDFJAC),  WORK((5*N)*(2*H)+((N*l)*N  2) ) 

REAL*8  X(N),XD,YD,AA. 

REA.L*8  R,PHIMAX,PHIHIN,THEIAMAX,THETAMIN,PHI,TBETA 
REAL*8  ![B,YB,ZB,SS0,RR,HAGNX,HAGN1,0Q,PI 
REAL*8  RAD,TX,GAMMA,,DPSI,DPHI,OT,OTX 

EXTERNAL  PDMA_ARM 

REAL*8  OTTPOP,OOP,T6|4,4l 
INTEGER  I,  J,  K 

REAL*8  TDES(4,4),  QHAX(6),  QHIN(6),  SCALE,  DANGLE,  DLENTH,  Hll! 
COMMON/LEN/  PI,R,T6,THETAU,THETAL,TTP 
COMMON  mm  I  DANGLE,  DLENTH, TDES 
COMMON  /KIN/  DT1,DT2,DT3,DT4,DT5, 

&  AL1,AL2,AL3,AL4,AL5, 

&  AA1,AA2,AA3,AA4,AA5, 

k  DD1,DD2,DD3,DD4,DD5, 

k  BL1,BL2,BL3,BL4,BL5, 

k  XK,Y»,ZW, 

&  DF6,TH6,SI6,PX6,PY6,PZ6 


RAD^25.40d0,2.0d0 

PIH.OdO*DATAN(1.0D01 
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C  Initialize  data  variables 


OBS=0 


C  Open  data  files  for  input 


OPED  (10,  HAHE='PDM.i-SOIJi.DAT',  STATCS='NE»' ) 
OPEK  1 9,  IKPUT.DAT',  STATES^'OLD' i 

C  Read  input  kineiatic  data 


READ 

(9,*) 

READ 

(9,*) 

rW,YW,2V 

RLAD 

(9,*) 

DT1,DD1,A.A1,AL1,BL1 

READ 

(9,*) 

DT2,DD2,AA2,AL2,BL2 

READ 

(9,M 

DT3,DD3,AA.3,AL3,BL3 

READ 

(9,*) 

DT4,DD4,AA4,AL4,BL4 

RLAD 

(9,*) 

DT5,DD5,AJi.5,AL5,BL5 

RLAD 

(9,*) 

READ 

(9,*l 

DF6,TB6,SI6,PX6,PY6,P;6 

READ 

(9,*) 

READ 

19, *1 

NOBS ,  QP ,  DANGLE ,  DLENTB ,  H.AGNV ,  HAGKl 

CLOSE 

(9) 

C  Adjust  noiinal  values 


XW^W-DLEKTE 

y^^W-DLENTH 

DT2^DT2+DAHGLE 

DT3=DT3+DA1IGLE 

DT4=DT4+DANGLE 

DTS^DTStDAUGLE 


AL1=AL1+DAHGLE 

AL2=AL2rLANGLE 

AL3=AL3+DA»GLE 

AL4=AL4+DAJfGLE 

AL5=AL5+DANGLE 

AAl=AAl+DLEirrH 

AA2=AA2+DLENTB 

AA3^AA3+DLEirrH 

AA4=AA4+DLENTH 

AA5=AA5+DLENTH 


DDl^DDl+DLENTH 

DD3=DD3*DLENTE 

DD4-DD4+DLENTE 
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DD5=DD5^DLENTH 


BL2=BL2+DANGLE 


DF6=DF6*DA})GLE 

PX6=PX6+DLE}nE 

PZ6=P26+DLENTH 

C  Set  liiits  on  spherical  coordinates 

PHIH>.X=90.0 

PHIHIH-0.0 

THETAH>.X=0.0 

THETAmN=360.0 

C  Get  randOB  niuber  seed 

WRITE  (6,*)  'Type  in  a  6-digit  randoB  nuiber  seed' 
READ  (5,*)  ISEED 

C  Start  of  Bain  loop 

1010  OBS=OBS*l 

C  Set  joint  angles  to  zero 

DO  M,N 
XlIl=0.0D0 
EHDDO 

C  Get  randoB  spherical  coordinats  for  end  effector 

1000  CALL  RANDOM  ( ISEED, NUHi 

PHI=PflIHIN+(PHIHAX-PHIHIN)*NL'H 
CALL  RANDOM  ( ISEED, NUMl 
THETA=THETAMIN+ ( THETAHAX-THETAHIN ) *NUM 
CALL  RANDOM  ( ISEED, NUHl 
Q=  100. 0+900. 0*NLT1 

C  Calculate  end  point  of  the  lanipulator 

XB=Q*COSD( THETA )*SIND(  PHI) 

YB=Q*SIND  ( THHA )  *SIND  ( PHD 
ZB=Q*COSD(PHD 

IF  (ZB  .LT.  50.0)  GOTO  1000 

C  Calculate  unit  vector  between  base  and  end  effector 


nZ=DSQRT(XB**2+YB**2+ZB**2) 

dxyz(1)=-xb/xy: 

DXYZl2l=-YB/XYZ 
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Cnz(3)=-2B,XY: 


C  Calculate  direction  angles  froa  direction  cosines 

DALPHA=DACOS(CXy:(l)) 

DBETA=DAC0S(DXY:(2)) 

DGAMHA=DAC0S{UXY;(31) 

C  Perturb  direction  angles 

33  CALL  RANDOM (ISEED, NTH  I 

ANUM=  ( 0 . 50D0-KUM )  *PI  /  6 .  ODC' 

RUXYZ ( l)=DCOS( DALPHA+ANUH ) 

CALL  RANDOM (ISEED, HUM) 

ANUM=(0.50D0-NUH)*PI/6.0D0 

RDXYZ ( 3 ) =DCOS ( DGAMHA+ANUM I 

CHECK=RUXYZ(1)**2+RCXY:(3)**2 

IF  (CHECK  .GT.  l.ODO)  GOTO  33 

PDIRCOS=DSQRT(  1 .0D0-RUXY2  ( 1 )  **2-RL'XY:  ( 3 )  **2 ) 

NDIRCOS=-PDIRCOS 

PNGLBTWN=DACOS ( CXYZ ( 1 ) *RCXYZ ( 1 ) +CXYZ ( 2 ) ‘PDIRCOS+UXYZ ( 3 ) 
i  *RUXYZ(3)) 

KHGLBTWN^DACOS ( DXY2 ( 1 ) *RCXYZ il ) +0XYZ ( 2 ) *NDIRCOS+OXY2 ( 3 ) 

&  *RCX'YZ(3)) 

RDXY2(2)=PDIRCOS 

IF  (DABS(PNGLBWH)  .GT.  DABS(NNGLBTWN) )  RDXY2(2)=NDIRCOS 

C  Establish  desired  tool  pose 

DO  11=1,4 
DO  JJ=1,4 

TDES(ii,iji=0.0 

EHDDO 

ENDDO 

TDES(1,3)=RUXYZ(1) 

TDES(2,3)=RCXY2(2) 

TDES(3,3)=RUXYZ(3) 

TDES(1,4)=XB 
TDES(2,4)=yB 
TDES(3,4)=ZB 
TDES( 4,4 1=1.0 

C  Call  IMSL  2XSSQ  for  inverse  kineiatic  solution 

NSIG=4 

EPS=0.0 

DELTA=0.0 

KAXFN=500 

IOPT=l 

nJAC=LDFJAC 
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CALL  I XSSQ  ( PL1LA_ARM ,  H ,  N , NSIG ,  EPS ,  DELTA , HAXFN ,  lOPT ,  PARK ,  X , 

&  SSQ ,  F ,  X  JAC ,  IXJAC ,  XJTJ  ,WORK ,  INFER ,  lER  i 

C  Check  for  singularities 

IF  (SSQ  .GT.  0.00001)  GOTO  1000 

C  Conpute  wire  length 

CALL  LENGTH (OTTPOP) 

C  Inject  noise  on  wire  length 

CALL  RANDOM! ISEED, NUN) 

OTTPOP=OTTPOP+ ( ( 0 . 5-NUH ) *2 . 0 ) *HAGNX 

C  Write  siiulation  data  to  file 

WRITE(10,*)OTTPOP 

WRITE  (10,888)  Xll),  X(2),  X(3),  X(4),  X(5),  X(6) 

888  FORMA.T  (  6F12.3  ) 

C  Continue  for  other  end  effector  positions 

IF  (OBS  .LT.  NOBS)  GOTO  1010 

CLOSE  (10) 

END 

Q  ii****tttt*Hiiit**tHi**tt*Hi1it*Hi**iii***ifkt*t**t**iiii**t1tHii1iii****i*ii 

SDBRODTINE  PUMA.ARH  (X,H,N,F) 

C  This  subroutine  calculates  the  non-linear  function  for  the  use  of 
C  the  IHSL  routine  2XSSQ,  It  is  the  forward  kinesatic  solution  for 
C  the  PUMA  lanipulator. 

INTEGER  H,  N 
REAL*8  X(N),  F(H) 

INTEGER  II,  JJ 

REAL*8  DTI,  DT2,  DT3,  DT4,  DT5 
REAL*8  DDl,  DD2,  DD3,  DD4,  DD5 
REAL*8  AAl,  AA2,  AA3,  AA4,  AA5 
REAL*8  ALl,  AL2,  AL3,  AL4,  AL5 
REAL*8  BLl,  BL2,  BL3,  BL4,  BL5 
REAL*8  DF6,  FI6,  TB6,  SI6,  PX6,  PY6,  PZ6 
REAL*8  XW,  YW,  ZW 

REAL*8  THl,  TH2,  TH3,  TH4,  TH5 

REAL*8  TO|4,4),  Tl(4,4),  T2(4,4),  T3(4,4),  T4(4,4) 
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Rm*8  T5i4,4l,  T6(4,4),  TRPY(4,4),  TXY;(4,4') 
REAL*8  TmTi4,4),  Tl4,4),PI,THETAU,THETAL,TTP 
RD1*8  DISQ,DIS,SUK 

nWEGEP.  I,  J,  K 

RDi*8  TDES(4,4),  DANGLE,  DLENTH,  R 


COmON.'LEN'  PI,R,T,TmAC,THETAL,TTP 
COMMON  /PDATA /DANGLE,  DLENTH, IDES 
COMMON  /KIN  DT1,DT2,DT3,DT4,DTS, 
i  AL1,AL2,AL3,AL4,AL5, 

&  AA.1,AA2,AA3,AA4,AA.5, 

A  DD1,DD2,DD3,DD4,DD5, 

A  BL1,BL2,BL3,BL4,BL5, 

A  Xlv,Y»;,2W, 

A  DF6,TH6,SI6,PX6,PY6,P:6 

C  Initiali2e  the  TIMAT  latrix  to  an  I  natrix: 

DATA  TIM.AT/1, 0,0, 0,0, 1,0, 0,0, 0,1, 0,0, 0,0,1/ 

C  Initialize  the  T  «atrix  to  an  I  latrix 
DO  II  =  1,4 
DO  JJ  =  1,4 

T(II,JJ)  =  TIM.iT(II,JJ) 

ENDDO 

ENDDG 

C  Manipulator  joint  angles 


THl  =  DTI  t  X(l) 

TH2  =  DT2  +  Xl2i 
THl  =  DT3  +  X(3l 
TH4  =  DT4  +  X(41 
TH5  =  DT6  +  X(5| 

FI6  DF6  +  Xl6| 

C  Coipute  the  T  latrices,  T1  thru  T6: 

CALL  T3XYZ  (  XK,  YW,  ZW,  TO  ) 

CALL  TRANSFORM  (  ALl,  AAl,  DDl,  THl,  BLl,  T1  ) 

CALL  TRANSFORM  (  AL2,  AA2,  DD2,  TH2,  BL2,  T2  ) 

CALL  TRANSFORM  (  AL3,  AAl,  DDl,  THl,  BLl,  T3  ) 

CALL  TRANSFORM  (  AL4,  AA4,  DD4,  TH4,  BL4,  T4  ) 

CALL  TRANSFORM  (  AL5,  AA.5,  DD5,  TH5,  BL5,  T5  ) 

CALL  T3RPY  (  FI6,  TB6,  SI6,  TRPY  ) 

CALL  TIXYZ  (  PX6,  PY6,  PZ6,  TXYZ  ) 

CALL  HA.TMULC  I  T6,  TRPY,  TXYZ  ) 
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C  Compute  the  overall  transformation,  T: 


CAU  HATHULl  (  T,  TO  | 

CALL  Hi.TK'L}.  (  T,  T1  ) 

CAU  MATHCLA  (  T,  T2  ) 

CAU  MATMULA  (  T,  T3  ) 

CAU  HATHCLA  (  T,  T4  i 
CAU  H.ATHl'Ii.  (  T,  T5  I 
CAU  HATHCLA  (  T,  T6  ) 

C  Calculate  the  function  F 

F(1)=T(1,4)-TDES(1,4| 
F(2)=T(2,4l-TDESl2,4) 
F(3)=T(3,4)-TDES(3,4i 
F|4)-(T(1,3)-TDES(1, 3)1*100. OM 
F(5)-(T(2,3)-TDES(2,3l)*100.0I>j 
F(6)  =  (T(3,3l-TDES(3, 3)1*100. ODC'. 


C  Calculate  residual 

SL'H^O.OW 
DO  IJKLU,6 
SUH=SUH*F(IJKL)*»2 
ENDDC' 

W!IT£i6,*)DSCRT(SL'Hi  '6.0DC 

RETCRh' 

END 

aJDPOCTINE  RANDOM  (x,2} 

C  This  subroutine  generates  random  numbers  in  the  range  0-1 
C  using  a  s.ipplied  seed  x,  the  returned  random  number  being  :. 

REAL  FH,  FX,  Z 
INTEGER  A,  X,  I,  M 
D.ATA  1,1/ 

IF  (  I  .EQ.  0  I  GO  TO  1000 

uo 

H-  2  **  20 

FH-  H 

A-  2**10  +  3 


1000  X-  MOD)  A*X  ,Hl 
FX-  X 
1--  FX  '  FH 
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RETUFJi 

END 

c 

C  This  subroutine  calculates  the  length  of  wire  between  the  end  effector 
C  fra«9  and  leasurement  base  fraae.  The  subroutine  utilizes  IHSL  routine 
C  ZXSSQ  for  solution  of  the  length  along  with  subroutine  HINLENTE 

SUBROUTINE  LENGTH (OTTPOP) 

REAL*8  T6,4,4),T6i*»r;i'4,4| 

Rm*8  EPSN,DELTAN,FARHKi4),XN(4),SSQN,FN(6),XJACNi6,4) 

REAL*8  WORKN!42),XJTJN(10),XlH”Tl4) 

INTEGER  HN ,  NN ,  NSIGN ,  HAXFNN ,  lOPTN ,  I XJACN ,  INFEPJ.' ,  lEPJi 
REAL*8  PI , R , X , Y ,  Z .  OC? ,  aLU\’ ( 4 1 , XUB'K  4 ) ,  XO , YO ,P6 1 4 ) ,  XOYO 
RLAL*8  X6Y6,Z0, OTTPOP, OTSCAL,OT,XUl 4 ),TTF,TTPSCAL 
REAL*8  THETAU,THETAL 
COHHON  lEN  PI,R,T6,THETAU,THET11,TT? 

EXTERNAL  HINLD-'TE 


C  Set  ZXSSQ  paraaeters 

HN=6 

NNM 

NSIGN--4 

EPS=O.ODO 

DELTAN=0.0[>C' 

HAXFNN =1000 

IXJACN=6 

IOPTN=l 

PI=4.0D0*DAT>Jfil.0I>;'i 

R=12.7OI>0 

C  Calculate  initial  values  for  ZXSSQ  vector  X 

X=T6(1,4) 

Y=T6i2,4i 

Z=T6(3,4) 

OOP=DSQRT(X**2-Y**2*Z**2l 

C  Calculate  unit  vector  froa  base  fraae  origin  to  end  effector  origin 

XLCYil)=X/OOP 

XLU-v(2)=Y/OOP 

XLCv’(3)=Z/OOP 

XLnYl4i=1.0DO 

C  Zero  T6  inverse  aatrix 
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DO  M,4 
DO  J=l,4 
TeiK-.'d.Ji-o.oD: 

ENDDC' 

ENDDO 

C  Coupute  inverse  of  T6  aatrix 

DO  M,3 
DO  J=l,3 

T6IN-;(I,J)d6!j,I) 

ENDDC' 

Teiir.’ddldeiN^'ddi-Teiididcd,!) 

T6IN';(2,4)=T6Iir;(2,4)-T6d,4l*T6d,2) 

T6IN';(3,4)=T6IN';(3,4l-T6d,4')*T6d,3i 

ENDDO 

T6IN'.'(4,4)d.OD3 


C  Calculate  coordinates  of  end  effector  unit  vector  in  leasureuent 
C  fra«e  coordinate 


Xri'vTddX-XLLTd) 

XDir.T(2)=Y-XLr;(2) 

XDr.?!3'ld-XLr;(3) 

XUL"-pi4)d.0D0 

C  Convert  coordinates  to  end  effector  fraae  reference 

DO  Id, 4 
XlTVdid.OD; 

DO  J=l,4 

xcLT  ( I)  dur,'  (I )  d6ifr;d ,  j  I  *  (  xuitp  d  i  i 

ENDDO 

ENDDC' 

C  Initialize  USSQ  X  vector 

XNdidLr;d'i*F 

XNi.,-XHrv’(2)*R 

XNl3)ddr,'d)*R 

XN(4)dDU'v(2)*R 

C  Call  ZXSSQ  for  length  calculation 

CALL  ZXSSQ(HIHLENTB,HN,KN,NSIGN,EPSN,DELTAN,HAXFNN,IOPTN, 
PARHN,XN,SSQN,FN,XJACNdXJACN,XJTJN,WORKN,INFERN,IERNl 

OTTPOP= ( THETAL+THETAD ) *R+TTP 

RETURN 

END 
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C 

C 

SDBROUTINE  HIHLENTH(XH,HH,ffi,FN) 

INTEGER  MN,NN 

RL\L*8  XN(NN),FN(KN), ERROR 

REAL*8  PI,R,X,Y,Z,OOP,XHrv(4),XUr;(4),XO,YO,P6(4),XOYO 
REAL*8  X6Y 6 , ZO , OPTPSCAL , OPT , OTSC AL , OT , XU ( 4 ) , TTP , TTPSCAL 
RD1*8  T6  ( 4 , 4 ) ,  P6P  ( 4 )  ,THETAU ,  THETAl  ,MAGUL , HAGUE ,  CXO 
RLAL*8  DY0,DZ0,XUPi4),UX6,CY6,DZ6,nXT,CYT,CZT,Z0P 
COMMON ;LEN /  PI , R , T6 , THE! AC , THETAL , TTP 

C  Initialize  variables  based  on  current  value  of  X 

XO^XN(l) 

Y0=XNi2! 

P8(ri=a’;31 

P6l2')-XN(4l 

P6P(l)^P6fll 

P6Pl2)=P6i2i 

C  Calculate  length  in  each  fraaes  xy  plane 

X0Y0=DSQRT(X0**2^Y0**21 

X6Y6=DSQRT(P6(1)**2-P6i2l*«2l 


C  Calculate  corresponding  z  value 

P6 ( 3 ) =DSQRT ( 2 . ODO*R*  X6Y6-P6 ( 1) **2*P6 ( 2 1 **2 1 
20=DSQRIi2.0DO*R*XOYO-XO**2-YO»«2) 

C  Calculate  angle  theta  for  arclength  calculations 

THETAC=DASIN(P6(3)/Rl 
THET.AL-DASINi  ZO  Ri 

C  Calculate  intenediate  z  value  for  unit  tangent  vectors 

P6P(3 i=X6Y6*DTAN(PI/2.0D0-TIETAU I 
ZOP=XOYO*DT\NlPI '2.0D0-TEETAL) 

MAGCL^DSQRTI XO**2+YO**2^ZOP**2 ) 

MAGDU=DSQRT I P6P( 1 ) **2+P6P| 2 ) **2^P6P( 3 i**2 I 

DX0=X0/MAGCL 

OYO-YO/MAGUL 

DZ0=20P/HA.GUL 

C  Transfon  unit  vector  in  FE  coordinates  to  FH  coordinates 

P6P(4)-1.0D0 
DO  M,4 
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Xl'P(I)<i.ODO 
DC'  J=l,4 

XUP(I)=XUPiI)^T6iI,Jl*P6PiJ) 

ENDDC' 

ENDIX) 

C  Transfon  FE  tangent  point  to  FH  coordinates 

Dx6=(xrp;i)-T6(l,4'il  nm 
CV6=(XTPi2l-T6i2,4)l/H.VGLT 
DZ6-fXUPi3l-T6(3,4|)/H.l.GLT 
P6|4)=1.0D': 

DO  1=1,4 
XClIl=0.0DC 
DC  J=l,4 

XC(Ii=XUiIl-T6lI,Jl*PC,J! 

ENDDC 

ENDX 

C  Calculate  tangent  to  tangent  distance 

TTP=DSQPT I ( XC ( 1 ) -XO ) **2- ( XU ( 2 ) -YO I **2*  i  XU !  3 1 '20 ) 

C  Calculate  tangent  point  to  tangent  point  unit  vector 

CXT=fXUfl)-XO) 'TIP 
UiT=(XU(2:-Y':'),TTP 
c:t=(xui3)-:o)  'ttp 

C  Calculate  Biniuiiing  functions 


FNlll=UXT-UXO 

FN'2l=UYT-UVr' 

fni3i=u;t-u;o 

FNi4i=CXT'U)>: 

FNI5)=U'YT*UY6 

fn(6i=u:T'U;6 


C  Calculate  residual  and  write  to  screen 

ERROR  =0.01X3 
DO  1=1,6 

ERROR=ERROR  +  FNll)**2 
ENDDO 

WRlTE(6,*lERROR 

RETURN- 

END 
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APPENDIX  E 


PROGR.iJ'.  k'llx 


C  Robot  Identification  using  the  Non-linear  Least  Squares  lethod. 

C  This  version  of  prograi  ID6  is  for  the  wire  potentioaeter  aethod. 
C  Siaulation  data  is  read  for  the  PUM.1  lanipulator  froc 
C  the  data  file  PLW.-SOU.DAT 
C 

C  SET  LDFJAC  =  NUMBER  OF  OBSERVATIONS 


INTEGER  LDFJAC,  HH,  H,  NN,  N,  NSIG,  MA.XFK,  lOPT,  IXJAC,  INFER, 
INTEGER  I,  J,  K,  NOBS,  XAXNOBS 
PARAMETER  (LDFJAC^BB,  MH=LDFJAC,  NN=24! 

PAPJXETER  (MAXN0BS=2C>01 

R£A.L*8  FJAC(LDFJAC,NNl,  XJTJ(  (NN-l  i*N'N  2) 

RLAL*8  PAPX:4l,  Fl LDFJAC),  WOPi^i  i5*NNi-(2»HMi-i  (NN-ll*NN  2): 

RLAL*S  X  ( NN  I ,  XD ,  YD , TX , DPSI , GAJWA. , DPEI  ,OT , RA.D 

RLAL*8  DAJiGLE,  DLEKTE,  TQ,  DC,  EPS,  DELTA,  SSC 

R£A.L*8  SQERP.l,  SQERR2,  PI 

RLAL*8 

R£A.L*8  DTI,  DT2,  DT3,  DT4,  DT5 
REAL»S  DDl,  DDL,  DD3,  DD4,  DDE 
RLAL*8  A.A1,  AA.2,  A.A3,  AA.4,  A_A5 
RLAL»8  ALl,  ALL,  AL3,  AL4,  ALE 
REAL»8  BLl,  BL2,  BL3,  BL4,  BLE 
REAL*8  FI6,  DF6,  THo,  SI6,  PX6,  P'io,  PLc 
RL*i*8  KA.GNX,M.AGN1 

REAL*8  TETI1H.AXNOBS),  TETLiMAXNOBS),  TET3(HA.XNOB3i 
REAL*8  TET4(H.AXNOBSl,  TETEIMA.XNOBS  i ,  TET61M.AXHOBS1 
REAL*8  R,OTXlMAXNOBS) 

COMMON  /PDATA'  NOBS,TETi,TET2,TET3,TEI4,TET5,TET6,OTX,PJJ? 


& 
i 
& 

& 

& 

& 

EXTERNAL  PUH.A_ARH 


COMMON  /KIN'  DT1,DT2,DT3,DT4,DTE, 
ALI,AL2,AL3,AL4,AiE, 
AA1,AA2,AA3,AA4,AA.5, 
DD1,DD2,DD3,DD4,DD5, 
BL1,BL2,BL3,BL4,BL5, 
XN,W,Ziil, 

DF6,TH6,SI6,PX6,PY6,PZ6,PI 


C  Open  data  files  for  inputs  and  results 

OPEN  (8,  N.AHE= 'RESULT. DAT',  STATUS='NEi»' ) 


OPEIi  (9,  N.W='PUK.i-SOLN.DAT',  STATL'S='OLD' I 
OPEJf  (10,HAHE='INPUT.DAT',  STATUS^ 'OLD' I 


PI^4.0D0*DATAJf!l.0D0l 

RAD=12.70D0 

c  Read  input  paraneters 

READ  (10, M 
RDD 

RLiJ)  (10,*)  DT1,DD1,AA1,AL1,BL1 
READ  (10,*)  DT;,DD2,AA2,AL2,BL2 
READ  (10,*)  DT3,DD3,EA3,AL3,BL3 
READ  (10,*)  DT4,DD4,AA4,A14,BL4 
READ  (10,*)  DT5,DD5,AA5,AL5,BL5 
READ  (10,*i 

READ  (10,*!  DF6,TB6,SI6,P);6,Pi'6,F;6 
READ  (10,*1 

REiD  (10,*1  NOBS, OPR, DAJJGLE,DLFNTH,H.AGNV,H.AGN1 
CLOSE  llOi 

C  Initiaii-e  data  variables 

Xili=X» 

Xi  2 

Xf3)--XK 

Xi4’^AAl 

X(5l=ALl 


Xi6'=D:2 

X(7|=A.A2 

X(8l=AL2 

X(9)-BL2 


XilOl-DTl 

XI111-DD3 

X|12)=AJ.3 

X(13l=AL3 

X(14)=:DT4 

X(15)=DD4 

X|16)=A>.4 

X(17)=AL4 

X(18)=DT5 

X(19)-DD5 

X(20)=AA5 

X(21)=AL5 
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X'22l-DFc 

X(23')=PX6 

X(24)^P;c 


C  Read  joint  data  and  wire  length. 


DO  J  =  1,  NGBo 

RE.iD  19, *1  TETKJ),  TET2(Ji,  TET3iJi,  TEI4(J),  TET5(J),  TET6(Jl 
READ  (9,*)  OTXiJl 
(>TXiJ|:^CXTX|J)*25.40D;' 

RE-iD  (9,*i 
ENDX 
CLOSE  (9) 

C  Set  paraieters  for  IHSL  routine  :XSSS  for  non-linear  identification 

NSIG=4 

EPS-C.O 

DLLTA=:0.0 

HAXFN=10j: 

IOPT=l 

ixjac=ldf:ac 

H=N0BS 


CALL  ■  XSSQ ,  PLK.AJJiP , HP ,  (Hi , NSIG ,  EPS , DELTA  ,K.AXrN ,  lOPT , 

4  PxiH ,  X ,  SSQ ,  F  ,  FJ  AC ,  I X  J.AC ,  XJTJ ,  WORE; ,  INFER ,  lEF, ' 


C  Save  results  to  data  file  ''RES"LT.DAT' 

WRITE  i?,*l 

WF.ITE  1 8,*!  'x»,  r.,  ;w' 

WRITE  18,93)  Xlll,  Xi2i,  Xi3) 

WRITE  iS,*l 

WRITE  1 8,* I  'DTI,  DDl,  A.A1,  .‘il,  BLl' 

WRITE  (8,93i  0.0,  0.0,  Xi4i,  X(5l,  0.0 
WRITE  (8,»i 

WTITE  (8,*)  'DT2,  DD;,  .‘-A2,  AI2,  BL2' 

WRITE  (8,931  X(6),  0.0,  X(7i,  X(8),  X(9i 
WRITE  (8,*) 

WRITE  18,*)  'DT3,  DD3,  AA3,  AL3,  BL3' 

WRITE  (8,93)  X(IO),  X(ll),  X(12),  X(13),  0.0 
WRITE  (8,*) 

WRITE  (8,*)  'DT4,  DD4,  AA4,  AL4,  BL4' 

WRITE  (8,93)  X(14),  X(15),  X(16),  X(17|,  0.0 
WRITE  (8,*) 

WRITE  (8,*)  'DT5,  DD5,  AA5,  ALB,  BL5' 

WRITE  (8,93)  X(18),  X(19),  X(20),  X(21),  0.0 
WRITE  (8,*) 

WRITE  (8,*)  '  DF6,  T16,  SI6,  PX6,  PY6,  PZ6' 
WRITE  (8,93)  X(22),  0.0,  0.0,  x(23),  0.0,  x(24i 
93  F0RHA.T)2X,6(1X,F10.4|) 
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WPITE  18, *1 

ifRITE  I8,*l  'INFER,  IER,NOBS,NSIG' 
BEITE  (8,*)  INFER,  IER,NOBS,NSIG 
WRITE  (6,*j  'INFER,  IER,NOBS,NSIG' 
WRITE  (6,*)  INFER,  IER,NOBS,NSIG 
WRITE  1 8,* I 

CLOSE  (8 1 


ENG 


^  **«*«*«** t«t**tttt*t«t*t*t*i*iit**x**ir******t***tt*«x***«*i^itxt«iiit»vs«* 


SUBROUTINE  PUKLARH  (X,  H,  N,  Fi 


C  This  subroutine  calculates  the  non-linear  function,  for  the  use  of 
C  the  IM3L  routine  ZXSSQ.  It  is  the  forviard  kine«atic  solution  for 
C  the  PUHX.  lanipulator. 


INTEGER  H,  N 
INTEGER  II,  J: 

INTEGER  I,  J,  K,  NOBS,  Kl.XNOSS 

PARAMETER  IH.i)j;'BS=20: 1 

RLAL*6  X(N),  FiMi 

RE.iL*8  XW,  T*,  :w 

R£A1*8  DTI,  DT2,  DT3,  DT4,  DT5 

REAL*8  DDl,  DU2,  DD3,  DD4,  DD5 

RLAL*S  AAl,  AA2,  AA.3,  AM,  M.5 

RLiL*8  ALi,  A:2,  AL3,  AL4,  AL5 

REA.L*8  BLl,  BL2,  BL3,  BL4,  BLS 

REAL*8  FI6,  DF6,  TH6,  SI6,  PX6,  Pit,  Pit 

RLAL*8  THl,  TH2,  TH3,  TH4,  TH5 

RLAL*8  TOM, 4),  TIM, 41,  T2M,4l,  T3M,4),  T4M,4i 

RLXL*8  T5I4,4),  T6(4,4),  TRPYM,4),  TXY:(4,4t 

REAL*8  TmTi4,4i,  TM,4 1 ,XT,YT,'T,XC,YC,AA,Q 

REAL*8  XD,YD,DPSI,RAD,GAHHA.,DPEI,OT,TX,OX 

RE.iiL*8  TETl(H.iXNOBSi,  TET21HA.XNOBS) ,  TET3(MAXNOBSi 

RLAL*8  TET4(KAXNOBS'l,  TET5(MAXNOBSl ,  TET6(HAXN0BSi 

RLXL*8  RE  ,OTX(MA.XNOBS I, PIK, ns, SUBsq 

REAL*8  OTTPOP,PI,R,OOP,THmU,THETAL,TTP 

COMMON  /PDATA;  NOBS, TET1,TET2,TET3,TET4,TET5,TET6,0TX, RAJ) 

COMMON  /KIN/  DTl,Dr2,DT3,DT4,DT5, 

&  AL1,AL2,AL3,AL4,AL5, 

&  AA1,AA2,AA3,AA4,AA5, 

i  DD1,DD2,DD3,DD4,DD5, 

&  BL1,BL2,BL3,BL4,BL5, 

&  XW,fW,2W, 

&  DF6,TH6,SI6,PX6,PY6,P26,PI 
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C  Initialize  the  TIHl.T  Batrix  to  an  I  latrix: 
DATA  TIH.AT,  1,0, 0,0, 0,1, 0,0, 0,0, 1,0, 0,0, 0,1 
PIK=PI 

C  Set  paraaeters  for  the  lanipulator: 

X*  =  Xili 

-  vi2, 

ZK  =  X(3i 

A.A1  =  Vi4; 

All  -  Xi5i 

DT2  =  Xiti 
A.A2  =  Xi'i 
AL2  =  > 

BL2  =  ).,?i 

dt:  =  Xiiv' 

DD3  ^  ),ill  i 
H2  --  hl2i 
AL3  =  X(13i 

074  =  Xil4i 
DD4  =  Xil5 
A.44  ^  M6i 
AL4  =  XiT) 

PT5  =  XilS; 

DD5  =  X(l^] 

A.A5  -  Xl2.i 
AL5  =  Xi21l 

DF6  -  Xi22i 
PX6  ^  X(23i 
P:6  =  X(24l 

C  Loop  NOBS  tiles 

K  =  0 

DO  J  =  1,  NOBS 

C  Initialize  the  T  latrix  to  an  I  latrix 

DO  II  =  1,4 
DO  JJ  =  1,4 

T(II,JJ)  =  TIHAT(II,JJ) 

ENDDO 

ENDDO 
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C  Manipulator  joint  angles 


THl  =  DTI  +  TETKJi 
TH2  =  DT2  +  TET21J) 

TH3  =  DT3  +  TET3(J) 

TH4  -  Dr4  +  TET4(J) 

TH5  =  DT5  +  TET5(J) 

FI6  =  DF6  +  TET6(j) 

C  Coipute  the  T  wtrices,  T1  thru  T6: 
aiL  T3xy;  (xw,yw,zk,to) 


CALL  TRANSFORM  ( 

ALl, 

AAl, 

DDl, 

TBl, 

BLl, 

T1 

CALL  TRANSFORM  ( 

AL2, 

A.A2, 

DD2, 

TH2, 

BL2, 

T2 

CALL  TRANSFORM  ( 

AL3, 

AA3, 

DD3, 

TH3, 

BL3, 

T3 

CALL  TRANSFORM  ( 

AL4, 

AA4, 

DD4, 

TH4, 

BL4, 

T4 

CALL  TRANSFORM  ( 

AL5, 

AA5, 

DD5, 

TH5, 

BL5, 

T5 

CALL  T3RPY  (  FI6 

/  TH6, 

,  SI6, 

,  TR?' 

i  ) 

CALL  T3XY:  (  PXt 

,  PV6, 

,  P26, 

,  txy: 

:  ) 

CALL  HATHULC  (  T6,  TRPV,  IXYZ  i 

C  Coipute  the  overall  transfonation,  T: 

CALL  KATHCLA.  (  T,  TO  ) 

CALL  K.ATKL’LA  (  T,  T1  I 
CALL  MATHULA  (  T,  T2  I 
QLL  MATMULA  (  T,  T3  i 
CALL  H.iTML’L4  f  T,  T4  ) 

CALL  MATHULA  (  T,  T5  ) 

CALL  MATMULA  (  T,  T6  I 

C  Calculate  the  "noiinal"  wire  length  based  on  current  paraieter  values 
CALL  LENGTH (OnPOP,T) 

C  Calculate  the  function  F 

F(J)-DABS(OTTPOP-OTX'(J) ) 

C  End  the  do-loop  for  counter  J 
ENDDO 

C  Coipute  RMS  error 


SDMS(J=O.ODO 
DO  J^l,HOBS 
SDMSQ=SDMSQ^F(J)*F(J) 
ENDDO 
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RMS^DSQRKSUMSQ'NOBS! 

liRITE(6,*)RHS 

RETURN 

END 

C  ii,iiitii*itHtitit1iit1ii**ii****i*****iHiii*it*i1i****t***i**t*t*HitHt**Hc*i*i**i 

C  This  subroutine  calculates  the  length  of  wire  froB  the  base  fixture  to 
C  the  lanipulator  endpoint  fixture  based  on  the  current  nanipulator 
C  endpoint  pose  (T6  =  T).  This  subroutine  uses  a  renaied  version  of  IHSL 
C  routine  ZXSSQ  (ZXSSQll  to  liniiize  the  sum  (conponent  by  conponent)  of 
C  unit  vectors  describing  the  tangent  points  for  both  upper  and  lower 
C  fixtures.  Subroutine  ZXSSQl  utilizes  subroutine  HINLENTH  to  evaluate 
C  the  'F"  functions. 

SDBROUTIHE  LENGTH (OTTPOF,T| 

REAL*8  T6(4,4),T6IK';(4,4),T(4,41 

REAL*8  EPSN , DELTAN , PARHN ( 4 ) , XN ( 4 1 , SSQN , FN ( 6 1 , XJACN 1 6 , 4 ) 

REAL*8  HORKN(42),XJTJN(10),XCU'vTi4) 

INTEGER  HNN , NNN , NSIGN , HAXFNN , lOPTN , I XJACN , INFEPH , lEPH 
REAL*8  PI,R,X,Y,2,OOP,XLir;(4),XLTr;(4),XO,YO,P6(4|,XOYO 
REA.L*8  X6Y6 , 20 ,OTTPOP ,OTSCAL,OT , XU( 4 )  ,TTP,TTPSCAL 
REAL*8  THETAU,TBETAL 
COmON  TEN,  PI,R,T6,THET.AU,TEET.AL,TTP 

EXTERNAL  HINLENTH 
DO  1=1,4 
DO  J=l,4 
T6(I,J)=T(I,J! 

ENDDC' 

ENDDO 

HNN=6 

NNN=4 

HSIGN=4 

EPSN=0.0D3 

DELTAN=0.0D0 

HAXFNN=1000 

IXJACN=6 

IOPTN=l 

PI=4.0D0*DAT>J)(1.0D0i 

R=12.70D0 

X=T6{1,4) 

Y=T6(2,4) 

Z=T6I3,4) 

OOP=DSQRT(X**2+Y**2+Z**2l 

XLDV(l)=X/OOP 

XLDV(2)=Y/OOP 

XLDV(3)=2/OOP 

XLLn/’(4)=1.0D0 

DO  M,4 
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DC'  J^l,4 

T6I!rv’(I,J)-0.0D0 

ENDDC' 

ENDDO 
DO  M,3 
DO  J^l,3 

ENDDO 

T6I[r;il,4)=T6Iir.’(l,4)-T6iI,4)*T6(I,n 

T6IN';(2,4)"T6rKv’(2,4)-T6(I,4)*I6(I,2) 

T6IN'C(3,4)=T6IN’;(3,4)-T6(I,4)*T6(I,3) 

ENDDO 

T6IN';(4,4)=1.0D0 

mT(i)=x-XLr:in 

XUL"T(2)=Y-XLr:i2) 

XDUVT(3)=Z-XLC’;i3) 

XDr;Pi4l^l.OD3 
DO  1=1,4 

xur;!n=o.oDo 

DO  J=l,4 

xuuv  ( I ) =xur;  ( ii  *T6  iir;  1 1 ,  j  1  *  (  xlt.t  i  j  i  i 

ENDDO 

ENDDO 

XNil)=XLU\'(l)*P. 

XNi2l=XLC’;(2l*P 

XN(3)=XUU\'lll*P 

XNi4)=XUl''v’(2)*P 


C  Call  renaied  version  of  ZXSSQ 

CALL  : XSSQ 1 ( MINLENTH , HNN , NNN , NSIGN , EPSN , DELTAN , MAX  FNN ,  lOPTN , 
i  PARHN,XN,SS(3N,FN,XJACN,IXJACN,XJTJN,WORKN, INFERN, lEPN' 

C  Calculate  wire  length 

OTTPOP=  ( THnAL+THETAL )  *P-TTF 

RETURN 

END 


SOBRODTINE  MINLENTH (XN, HNN, HNN, FN) 

INTEGER  HNN, NNN 

REAL*8  XN{4),FN(6), ERROR, AAA,BBB 

REAL*8  PI,R,X,Y,Z,OOP,XLD\M41,XDCV(4I,XO,YO,P6(4),XOYO 

R£AL*8  X6Y6 , ZO,OPTPSCAL,OPT ,OTSCAL,OT ,XD( 4 ) ,TTP,TTPSCAL 
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RLiL*8  T6! 4 , 4 ) ,P6Pi 4 , ,THETAC ,THETAL,Ki.GUL,M.\GCU,U);0 
RLAL*S  CY0,D20,XnP(4),CX6,CV6,CZ6,UXT,Un,C:i,Z0P 

COMMON  lEN '  PI,R,T6,TEETAL',THET.il,TTP 


XO=XNil) 

Y0=XNi2) 

P6(l)=XNl3l 

Pb(2)=XN'4i 

P6P(l)=P6.'l) 

P6P(2)=P6;2i 

XOYO=DSO»T(XO**2+YO**2) 

X6Y6^DSQPTiP6(l)**2-P6(2)**2) 

AAA=2 . ODO*R*X6Y6-P6 ( 1 ) **2-P6( 2 1 **2 
BBb=2.0DO*R*X0Y0-X0**2-Y0**2 
IF  (AAi  .LT.  O.ODD)  THEN 
P6l3)=R 
ELSE 

P6(3)=DSQRTi>Jil 

ENT)IF 

IE  (BB5  .LT.  O.ODO)  THEN 
ZO=R 
ELSE 

ZO=DSQRTiBBB) 

EKDIF 

THETAU--DASIN(P6(3l  Rl 
THETAL=DASIN{ZO'R) 

P6P 1 3 ) =X6Y6*DT.iJI I  PI  2 .  ODO-THET.AU  i 

ZOP= XOYO*DTAN ( PI /2 . ODO-THET AL i 

HAGUL=DSQRT(  X0**2*Y0**2<0P'*2 1 

MAGUII=DSQRT(P6P(l)**2+P6P(2)**2*P6Pi3l**2l 

L’XO^XO  HA.GUL 

UYO^YO 'K.AGCL 

UZO^ZOP  H.AGUL 

C  TransforB  tangent  vector  in  FE  coordinates  to  FH  coordinates 

P6P(4)=1.0I>] 

DO  1=1,4 
XDP(II=O.OIX) 

DO  J=l,4 

xup(I)=xi:p(I)*t6i:i,J)*p6P(Ji 

EKDDO 

ENDDO 

C  Calculate  upper  tangent  vector  in  FH  coordinates 

nX6=(XDP(l)-T6(l,4))/HAGCU 

(IY6=(XUP!2)-T6(2,4))/HAG'JU 

DZ6=(XCPI3)-T6(3,4))/MAGLT 

P6(4)=1.0D0 
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DO  1=1,4 
XU(I)=O.ODO 
DO  J=l,4 

XU(I)=XU(I)+T6(I,J)*P6(J) 

ENDDO 

ENDDO 

TTP=DSQRT((XU(l)-X0)**2+(XD(2)-Y0)**2+(XU(3)-20)**2) 

0XT=(XD(1)-X0)/TTP 

UYT=(XU(2)-yO)/TTP 

DZT=(XU(3)-Z0)/TTP 

C  Calculate  *ini»izing  functions 

FH(1)=DXT-DX0 

F}I(2)=UYT-DY0 

FN(3)=DZT-DZ0 

FN(4)=UXT+DX6 

FN(5)=DYT+UY6 

FII(6)=UZT+UZ6 

C  Calculate  residual 

ERROR  =0.0D0 
DO  1=1,6 

ERROR=ERROR  +  FN(I)**2 
ENDDO 

WRITE (6,*) ERROR 

RETURN 

END 
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APPENDIX  F 


Q  **i1t**Hi*i**i1t***i*****Ht*t**i*1t*i****i*Hi*********tHti**it*****HiHit 


PROGRAM  Wv’ERIFi' 

C  This  prograB  generates  the  six-dof  pose  error  for  the  PUMA  aanipulator. 

C  It  contains  the  identified  calibration  paraieters  and  the  exact  paraaeter. 
C  It  uses  a  data  file  of  verification  joint  angle  sets  POSEVER.DAT,  and  the 
C  file  RESULT.DAT  froi  the  prograB  ID6. 

INTEGER  I,  J,  K,  MPOSES,  N 
REAL*8  DANGLE,  DLENTH 

REAL*8  DT(5),DD(5),AA(5),AL(5),BL(5),HEAS(6) 

REAL*8  EDT(5),EDD(5),EAA(5),EAL(5),EBL(5),EHEAS(6) 

REAL*8  EDF6 , EFI6 , ETH6 , ESI6 , EPX6 , EPY6 , EP26 
REAL*8  THETA(1000,6),  TDELTA(4,4) 

REAL*8  TO(4,4),  Tl(4,4),  T2(4,4),  T3(4,4) 

REAL*8  T4(4,4),  T5(4,4),  T6(4,4),  TRPY(4,4),  TXY2(4,4) 

REAL*8  TIHAT(4,4),  T(4,4),  ET(4,4) 

REAL*8  DTI,  DT2,  DT3,  DT4,  DT5 
REAL*8  DDl,  DD2,  DD3,  DD4,  DD5 
REAL*8  AAl,  AA2,  AA3,  AA4,  AA5 
REAL*8  ALl,  AL2,  AL3,  AL4,  AL5 
REAL*8  BLl,  BL2,  BL3,  BL4,  BL5 
REAL*8  DF6,  FI6,  TH6,  SI6,  PX6,  PY6,  PZ6 
REAL*8  XK,  YW,  ZW 
COMMON  TIHAT, THETA 

C  Initialize  the  TIHAT  latrix  to  an  I  latrix: 

DATA  TIMAT/1,0, 0,0,0, 1,0,0,0,0, 1,0, 0,0,0,!/ 

C  Open  data  file 

OPEN  (9,  NAME='POSEVER.DAT',STATDS='OLD') 

OPEN  (10,  HAME='INPDT.DAT',  STATDS='OLD') 

OPEN  (11,  MAME='RESDLT.DAT',  STATUS='OLD') 

C  Read  input  paraieters 

READ  (10,*) 

READ  (10,*)  HEAS(1),HEAS(2),KEAS(3),H£AS(4),I(£AS(5),MEAS(6) 

READ  (10,*)  DTI, DDl, AAl, ALl, BLl 
READ  (10,*)  DT2,DD2,AA2,AL2,BL2 
READ  (10,*)  DT3,DD3,AA3,AL3,BL3 
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READ  (10,*)  DT4,DD4,AA4,AL4,BL4 
READ  (10,*)  DT5,DD5,AA5,AL5,BL6 
READ  (10,* 

READ  (10,*)  DF6,TH6,SI6,PX6,PY6,P26 
READ  (10,*) 

READ  (10,*)  HOBS,R,DAMGLE,DLE!ni,MAGNX,HAGNL 
CLOSE  (10) 


C  Read  in  joint  anqle  sets  for  verification  poses 

MPOSES=NOBS 

DO  I=l,NPOSES 
READ(9,*) 

READ(9,*)THETA(I,1),THETA(I,2),THETA(I,3),TEETA(I,4), 
&  THETA(I,5),THETA(I,6) 

ENDDO 

CL0SE(9) 

C  Set  exact  link  paraieters  for  the  lanipulator: 

DO  1*2,5 

DT(I)»DT(I)+DAMGLE 

ENDDO 

HEAS(1)*HEAS(1) 

HEAS(2)=KEAS(2) 

HEAS(3)=HEAS(3) 

HEAS(4)=MEAS(4)*DLENTE 

HEAS(5)=HEAS(5)+DLENTH 

KEAS(6)*HEAS(6)*DLENTH 

AL(1)=AL1+DANGLE 

AL(2)=AL2*DANGLE 

AL(3)=AL3+DAHGLE 

AL(4)*AL4tDANGLE 

AL(S)*AL5*DANGLE 

AA(1)  *  AAl  *  DLENH 
AA(2)  =  AA2  +  DLENTH 
AA(3)  *  AA3  *  DLENTH 
AA(4)  =  AA4  +  DLENTH 
AA(5)  =  AA5  +  DLENTH 

DD(1)  =  DDl 
DD(2)  *  DD2 
DD(3)  =  DD3  +  DLENTH 
DD(4)  *  DD4  *  DLENTH 
DD(5)  =  DD5  +  DLENTH 
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BL(1)  =  BLl 
BL{2)  =  BL2  +  DANGLE 
BL(3)  =  BL3 
BL(4)  =  BL4 
BL(5)  =  BL4 

DF6  =  DF6  +  DANGLE 
TH6  =  TH6 
SI6  =  SI6 

PX6  =  PX6  +  DLENTH 
PY6  =  PY6 

PZ6  =  P26  +  DLENTH 

C  Read  in  and  set  up  estiiated  paraieter  table 


READdl,*) 

READdl,*) 

READdl,*)  EHEASd),EHEAS(2),EHEAS(3) 


DO  1=1,5 
READdl,*) 

READdl,*) 

READ  (11,*)  EDTd),EDDd),EAAd),EALd),EBLd) 
ENDDO 


READdl,*) 

READdl,*) 

READdl ,  * )  EDF6 ,  ETH6 ,  ESI6 ,  EPX6 ,  EPY6 ,  EP26 
C  Main  loop  through  NPOSES  joint  angle  sets 
DO  K=l, NPOSES 

CALL  FKS  (K,HEAS,DT,AL,AA,DD,BL,FI6,TH6,SI6,PX6,PY6,PZ6,T) 
CALL  FKS  (K,EHEAS,EDT,EAL,EAA,EDD,EBL,EFI6,ETH6,ESI6,EPX6, 
&  EPY6,EPZ6,ET) 

C  Coipute  the  differential  tool  latrix 

CALL  HATSDB(TDELTA,T,ET) 

c  Coipute  the  pose  errors 

POSERR=SQRT (TDELTA (1,4) **2+TDELTA( 2 , 4 )**2+TDELTA( 3 , 4 ) **2 ) 
MIERR1= ( TDELTA ( 3 , 2 ) -TDELTA ( 2 , 3 ) ) /2 
ORERR2= ( TDELTA (1 , 3 ) -TDELTA ( 3 , 1 )) /2 
ORERR3=(TDELTA(2,l)-TDELTA(l,2))/2 
ORERR=SQRT(ORERR1**2+ORERR2**2+ORERR3**2) 

c  Dpdate  total  error  counts 
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POSTERR= ( POSERR+ ( K-1 ) ‘POSTERR ) /K 
(»TERR= ( ORERR+ ( K-1 )*ORTERR ) /K 

c  End  of  lain  loop 

ENDDO 

WRITE(6,*)  'Position  error,  orientation  error' 

WRITE (6,*)  POSTERR,ORTERR 
EHD 

C  HttitiiUtHttiiUtHtiiHt******************************************** 

SDMOOTIHE  PKS  (ll,HEAS,DT,AL,AA,DD,BL,DF6,TE6,SI6, 
k  PX6,PY6,P26,T) 

REAL*8  TO(4,4),  Tl(4,4),  T2(4,4),  T3(4,4) 

REAL*8  T4(4,4),  T5(4,4),  T6(4,4),  TRPV(4,4),  TXY2(4,4) 

REAL*8  TIHAT(4,4),  T(4,4),  dt(5),al(5),aa{5),dd(5),bl(5) 
REAL*8  THETA(1000,6),A«G(5),HEAS(6) 

OOmCHt  TIHAT, THETA 

C  Initialize  the  T  latrix  to  an  I  latrix: 


DO  J=l,4 
DO  K=l,4 

T(J,K)  =  TIHAT(J,K) 
ENDDO 
ENDDO 

C  Set  up  the  joint  angles 
DO  1=1,5 

ANG(I)=THETA(N,I)+DT(I) 

ENDDO 

FI6=THETA(N,6)+DF6 


C  Coipute  the  T  Matrices,  T1  thru  T6: 

CALLT3RPY  (HEAS(1),HEAS(2),HEAS(3),T0) 

CALL  T3XYZ  (K£AS(4),KEAS(5),KEAS(6),T0) 

CALL  HATHDLC  (TO,TRPY,TXYZ) 

CALL  TRANSFORM  (AL(1),AA(1),DD(1),ANG(1),BL(1),T1) 
CALL  TRANSFORM  (AL(2),AA(2),D0(2),ANG(2),BL(2),T1) 
CALL  TRANSFORM  (AL(3),AA(3),DD(3),A1IG(3),BL(3),T1) 
CALL  TRANSFORM  (AL(4),AA(4),DD(4),AN6(4),BL(4),T1) 
CAU  TRANSFORM  (AL(5),AA(5),DD(5),ANG(5),BL{5),T1) 
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CALL  T3RPY  (FI6,TH6,SI6,TRPY  ) 
CALL  T3nZ  (PX6,PY6,P26,TXY2  ) 
CALL  HATHULC  (T6,TRPY,TXY2  ) 

C  Coipute  the  overal]  transfonation,  T: 

CALL  HATHULA  (  T,  TO  ) 

CALL  HATHDLA  (  T,  T1  ) 

CALL  MATHCLA  (  T,  T2  ) 

CALL  MATHULA  (  T,  T3  ) 

CALL  HATHDLA.  (  T,  T4  ) 

CALL  HATHULA.  (  T,  T5  ) 

CALL  HATHULA  (  T,  T6  ) 

RETDRM 

END 
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APPENDIX  G 


Two  of  the  more  important  aspects  in  robot  calibration  are 
precise  measurements  and  maximum  joint  excursions  during  data 
collection.  An  instrument  referred  to  as  a  linear  slide  is 
capable  of  displacement  measurements  accurate  to  0.01  mm. 
However,  restricting  the  end  effector  to  linear  travel  can 
severely  limit  joint  variation  for  one  or  more  manipulator 
joints.  In  fact,  there  are  a  number  of  configurations  in  which 
one  joint  may  not  vary  at  all.  The  purpose  of  this  project 
then  is  to  establish  a  position  and  orientation  of  the  slide 
which  will  maximize  joint  excursion  for  all  six  joints.  The 
project  will  require  a  dual  use  of  the  ADS  program  as  is 
described  below. 

A  number  of  methods  exist  for  developing  an  analytical 
approach  to  the  forward  kinematic  solution  for  a  manipulator 
(ie  given  a  set  of  joint  angles,  what  is  the  position  and 
orientation  (pose)  of  the  end  effector).  However,  an 
analytical  solution  to  the  inverse  kinematics  is  much  more 
difficult  or  impossible  to  develop.  Therefore,  the  first 
application  of  ADS  will  be  to  solve  this  nonlinear  problem  in 
the  following  manner.  The  design  variables  will  be  the  6  joint 
angles.  The  design  variables  are  bounded  by  the  physical 
bounds  on  their  rotations  and  additional  restrictions  to  limit 
the  robot  to  one  "arm  configuration".  The  design  variables  are 
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used  to  produce  a  forward  kinematic  solution  which  is  then 
compared  to  the  "desired"  pose.  The  goal  then  is  to  minimize 
the  error  between  the  "desired"  and  calculated  poses.  The  pose 
information  is  in  the  form  of  a  four  by  four  homogeneous 
transformation  matrix  which  is  calculated  in  the  following 
manner.  A  coordinate  frame  is  assigned  to  each  manipulator 
link  in  a  standardized  method.  Due  to  two  geometric 
constraints,  4  parameters  are  required  to  transform  from  link 
to  link.  These  parameters  include  two  rotations  of  which  one 
is  the  rotary  joint  angle  and  the  other  is  a  twist  angle,  and 
two  translations  which  are  essentially  the  link  length  and  an 
offset  distance.  Using  a  standardized  approach,  the 
transformation  takes  the  following  form: 

X 

,  b,  Cj  y 

•ii-l  =  . 

0  0  0  1 
=  f(a^,di,6^,aj) 

where  the  a^,  b^  and  c^  entries  are  direction  cosines  and  x,  y, 
and  z  entries  are  the  position  with  respect  to  the  i-1 
coordinate  frame. 

As  noted,  the  transformation  is  a  function  of  the  four 
parameters  and  in  this  application  the  nominal  values  are  used 
for  the  twist  angle  and  the  translations.  The  rotary  joint 
angle  is  the  variable  for  each  transformation  matrix.  If  the 
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frame  to  frame  transformation  matrices  are  multiplied  in  the 
following  manner,  then  the  manipulator  pose  (ie.  the  position 
and  orientation  of  the  manipulator  end  effector  with  respect 
to  a  "world"  coordinate  frame  will  be  given  by  the  T*  matrix: 

=  tI  tI  tI  tI  Tt  t!  tI 

To  compute  the  inverse  solution,  a  desired  pose  matrix  is 
formed  which  is  facilitated  in  this  application  by  orientating 
the  "world"  coordinate  frame  with  the  axis  of  the  slide  and  at 
its  zero  position.  The  design  variables  (the  six  joint  angles) 
are  given  an  initial  value  which  are  used  to  compute  a  forward 
solution.  The  difference  between  the  computed  T‘  matrix  and 
desired  T*  matrix  are  calculated  term  by  term.  Then,  the 
objective  function  is  formed  as  the  sum  of  the  squares  of  the 
element  by  element  differences.  Note  that  if  a  solution  exists 
(ie.  reachable  by  the  manipulator),  the  objective  function 
will  be  zero  (or  at  least  "small").  This  objective  function 
value  will  then  be  used  as  a  constraint  in  the  second 
application  of  the  ADS  program  to  ensure  the  slide  is 
"reachable"  by  the  robot.  The  following  is  a  mathematical 
statement  of  the  problem. 
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First  application  of  ADS: 


Design  varieQ^les: 

0x 

02 

_  e, 

^=0: 

05 

06 


Minimize: 

FiB  =  i,J  =  l,2,3,4 


where 


j 


c, 


-1.  J 


and  di,,  and  are  the  ijj""  entries  of  the  ’’desired"  and 
calculated  transformation  matrices. 

In  the  second  application  of  the  ADS  program,  the  design 
variables  are  the  x,y  and  2  position  of  the  end  of  the  slide 
as  well  as  two  orientation  angles  theta  and  phi  which  are 
azimuth  and  elevation  angles.  The  end  of  the  slide  must  lie 
outside  a  circle  of  radius  of  150  mm  from  the  joint  one  axis 
of  rotation  and  this  becomes  one  of  the  nonlinear  inequality 
constraints.  The  inverse  solution  for  six  poses  along  the 
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slide  will  be  computed  as  described  above  which  will  serve  two 
purposes.  As  stated  earlier,  it  will  constrain  the  problem 
within  the  "reachable"  range  of  the  manipulator.  This  will 
make  up  six  nonlinear  inequality  constraints.  Additionally, 
the  inverse  solution  provides  six  sets  of  joint  angles  for  one 
slide  position  and  orientation.  The  maximum  joint  excursion 
for  each  joint  is  then  determined  from  this  information.  The 
goal  is  to  maximize  the  excursion  of  all  six  joint  angles. 
Therefore,  objective  function  will  be  to  minimize  the  negative 
value  of  joint  one  range  over  slide  travel.  The  additional 
five  joint  excursion  ranges  are  compared  to  joint  one's  range 
and  these  form  5  additional  linear  inequality  constraints. 
Additionally,  the  maximum  displacement  of  the  slide  zero  point 
Was  placed  at  1000  mm  from  the  base  frame  of  the  robot  and 
this  made  up  the  thirteenth  constraint. 

Mathematically  stated: 

Second  application  of  ADS: 

Design  variables: 


X  = 


X 

y 

z 

0 

.<1>J 
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Minimize : 


Fi5()  =  -Ae^ 

where  delta  theta  is  the  maximum  range  of  joint  excursion  for 
the  six  positions  along  the  slide 
Subject  to: 


9^12  = 


= 

Ae, 

-Ae^ 

iO 

92 

= 

AO, 

-A  63 

^0 

9^ 

= 

Ae, 

-A04 

iO 

9i 

= 

Ae; 

-A05 

^0 

95 

= 

A0, 

-A0, 

^0 

96 

= 

^1- 

0.01 

^0 

9i 

= 

^2- 

0.01 

^0 

9e 

= 

^3- 

0.01 

^0 

9s 

= 

^4- 

0.01 

iO 

9\q 

= 

0.01 

lO 

9^1 

= 

0.01 

iO 

1 

fx 

:^+y‘ 

^'150 

.0 

150.0 


S  0 


9i3  = 


-1500 

1500 


iO 


where  Fi  is  the  value  of  the  objective  function  for  each  of 
the  six  positions  along  the  slide  as  calculated  in  the  first 
application  of  ADS  (inverse  kinematic  solution)  which  will  be 
referred  to  as  the  "inner  loop"  of  the  program. 

The  first  application  of  ADS  was  tested  independently. 
After  considerable  testing,  it  was  found  that  the  following 
combination  of  methodology  and  parameter  settings  performed 
"best" . 
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istrat 

iopt 

ioned 

dabob j 

dabobm 

dabstr 

fdch 

fdchm 

itmax 

scaling 


0 

3 

3 

0.001 

0.0001 

0.0001 

0.0001 

0.00001 

60 

off 


The  results  of  these  tests  were  compared  to  results 
obtained  from  a  well  tested  IMSL  routine  which  was  unsuitable 
for  implementation  within  the  "inner”  loop  of  the  main  program 
due  to  the  inability  to  bound  the  joint  angles  and  obtain  an 
objective  function  value  when  the  endpoint  was  not 
"reachable" .  The  chosen  method  consistently  converged  with  an 
accuracy  within  3  digits  of  the  IMSL  routine.  The  major 
drawback  was  the  average  of  200  function  evaluations  required. 
However,  accuracy  and  precision  were  crucial  so  the  large 
number  of  function  calls  was  a  necessary  tradeoff. 

With  the  first  ADS  application  developed  and  working,  this 
program  was  then  implemented  as  a  subroutine  within  the  second 
application  of  ADS.  This  program  was  tested  using  the 
recommended  methods  of  the  ADS  manual  and  additionally  with 
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one  dimensional  search  methods  2  and  6.  No  matter  what 
strategy  or  optimizer  chosen,  ioned  values  of  2  and  6  always 
failed  to  converge  (there  is  no  obvious  reason  for  this  other 
than  the  non-linearity  of  the  problem).  Additionally, 
strategies  3,  6  and  7  failed  in  all  configurations  attempted 
(ie  parameter  setting,  optimizer,  and  one  dimensional  search 
variations).  Of  the  variations  tested  that  converged,  the 
following  combination  of  methodology  and  parameter  settings 
worked  "best" . 

istrat  =  9 
iopt  *  5 
ioned  =  7 
scaling  off 
dabobj  =  0.001 
dabobm  =  0,0001 
dabstr  =  O.OOOl 

"Best"  in  this  setting  was  determined  by  convergence  and 
number  of  function  calls.  This  best  method  consistently 
converged  in  the  fewest  number  of  function  calls.  However, 
even  though  this  is  the  fastest  method,  the  program  still 
required  as  much  as  15-20  minutes  of  run-time  on  a  VAX  3100 
station  (30  minutes  or  longer  on  the  older  workstations).  As 
expected  (due  to  the  nonlinearity  of  the  problem),  there  are 
apparently  a  number  of  local  minima.  Several  starting 
positions  were  chosen  and  tested.  Printouts  of  the  results  for 
several  initial  values  are  enclosed.  The  following  lists  the 
optimum  of  the  local  minima  found.  From  the  geometry  of  the 
problem  this  result  is  probably  "close"  to  the  global  minimum 
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and  was  a  significant  improvement  from  the  initial  design 
values  and  the  design  used  in  the  actual  experiment. 

One  improvement  to  the  program  which  significantly 
improved  convergence  and  reduced  run-time  was  a  seeding  method 
used  for  the  inner  loop.  A  reasonable  set  of  initial  joint 
angles  was  chosen  prior  to  the  first  pass  through  the 
subroutine.  The  joint  angles  calculated  for  the  zero  position 
on  the  slide  were  then  used  for  initial  values  for  calculating 
the  joint  angles  of  the  second  position.  This  process  was 
repeated  for  each  additional  measurement  position.  After  the 
main  program  design  variables  are  varied,  the  initial  value  of 
the  joint  angles  are  set  to  the  previously  calculated  joint 
angles  for  the  previous  slide  zero  position  before  calling  the 
subroutine. 

Additional  testing  of  both  parameter  settings  and  initial 
values  should  be  performed  to  both  ensure  that  the  program 
performs  "optimally”  and  to  search  for  the  global  minimum. 
However,  the  results  at  this  stage  are  guite  satisfactory. 
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RESULTS 
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A©!  =42.8 
ASj  =  46.6 
Ae3  =  96.8 

Ae^  =  80.1 

ASg  =  42.8 
Ae^  =  45.7 


X  =  77 .9  Tm 
y  =  -431  mm 
z  =  248  mm 

e  =  193“ 
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Xq  =  -200  mm 
Yq  =  -200  mm 


Zq  =  400  mm 
00  =  135“ 


<l>o  =  45“ 


FUNCTION  EVALUATIONS  FOR  MAIN  LOOP:  120 


FUNCTION  EVALUATIONS  PER  INNER  LOOP  ITERATION:  AVERAGE  OF  200. 
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